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ABSTRACT 


This report advances a linear operator approach for analyzing the dynamics of systems of 
joint-connected rigid bodies. It is established that the mass matrix M for such a system can 
be factored as M = (I + H$L)D(I + H$L) T . This yields an immediate inversion M _1 = 
(J — H'&L) 7 ' D~ 1 ( I — iT'J'jL), where H and $ are given by known link geometric parameters, 
and L, $ and D are obtained recursively by a spatial discrete-step Kalman filter and by the 
corresponding Riccati equation associated with this filter. The factors ( I + H&L) and (J — H^L) 
are lower triangular matrices which are inverses of each other, and D is a diagonal matrix. This 
factorization and inversion of the mass matrix leads to recursive algorithms for forward dynamics 
based on spatially recursive filtering and smoothing. The primary motivation for advancing the 
operator approach is to provide a better means to formulate, analyze and understand spatial 
recursions in multibody dynamics. This is achieved because the linear operator notation allows 
manipulation of the equations of motion using a very high-level analytical framework (a spatial 
operator algebra) that is easy to understand and use. Detailed lower-level recursive algorithms 
can readily be obtained by inspection from the expressions involving spatial operators. The report 
consists of two main sections. In Part I, the problem of serial-chain manipulators is analyzed and 
solved. Extensions to a closed-chain system formed by multiple manipulators moving a common 
task object sire contained in Part II. To retain ease of exposition in the report, only these two 
types of multibody systems are considered. However, the same methods can be easily applied to 
arbitrary multibody systems formed by a collection of joint-connected rigid bodies. 
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Part I 

RECURSIVE MASS MATRIX FACTORIZATION AND INVERSION 
FOR OPEN-CHAIN SYSTEMS 



ABSTRACT 


In this paper, some of the power of an operator formulation for manipulator dynamics is demon- 
strated by readily obtaining several distinct recursive forward dynamics algorithms. Starting from the 
Newton-Euler Formulation of the equations of motion for an iV'-link serial manipulator with a fixed base, 
the manipulator dynamics are re-expressed in a linear operator form. Using an operator factorization 
and inversion technique, applied to the manipulator mass matrix M , an O(N) iterative forward dynam- 
ics algorithm is then obtained. Additional O(A) algorithms are derived by the use of certain operator 
identities. 

The extension to a fully mobile base is discussed in an appendix. The manipulator mass matrix is 
shown to have a factorization M = (J + H<&L)D{I + H®L) T yielding an immediate inversion M -1 = 
(7 - H^L) t D~ r (7 — H^L), where H and $ are given by known link geometric parameters, and L, $ 
and D are obtained via a discrete-step Riccati equation driven by the link masses. The factors (7 + H$>L) 
and (7 - H^L) are lower triangular matrices which are inverses of each other, and D is a diagonal matrix. 

1. INTRODUCTION 

In this paper, manipulator dynamics are given an operator interpretation which greatly facilitates 
the ability to manipulate and solve expressions arising from considerations involving robot kinematics, 
dynamics and control. Furthermore, the analytical results of such manipulations can often be mapped 
directly onto sin equivalent recursive algorithm. To illustrate this, the robot dynamical equations will be 
given an operator formulation, after which several recursive forward dynamics algorithms will be derived. 
In the body of this paper, the assumption is made that the base is fixed and that link joints are one degree 
of freedom revolute or sliding joints. In an Appendix B* the base is allowed to be fully mobile, and it is 
discussed how three degree-of-freedom spherical ball joints can be incorporated into the dynamics. 

The ability to quickly and iteratively compute the forward dynamics of an iV-link serial manipulator 
enables efficient simulation of arbitrary robot arms from knowledge only of the individual link geometric 
and mass properties and the nature of the link interconnections. An explicit analytical expression de- 
scribing manipulator dynamics is not needed [1]. An O(N) iterative algorithm for forward dynamics has 
been proposed in [2] and [3] by R. Featherstone. This algorithm is derived utilizing the Spatial Algebra, 
Kinematics, and Dynamics developed in [3] (which is related to the Motor and Screw Calculus). The 
algorithm of [2] and [3] first computes bias terms due to coriolis, centrifugal, gravity, and contact forces 
by means of the O(N) iterative Newton-Euler algorithm for inverse dynamics [4,5]. It then subtracts this 
bias from the input joint moments (in the manner previously suggested by [l]) to obtain a “bias- free” 
robot dynamics equation. After this step, the algorithm proceeds to obtain joint-angle accelerations by an 
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additional O(N) iteration. 

This paper extends the recent work of [6,7] to show that O(N) iterative forward dynamics algorithms 
can be easily obtained by the use of techniques for solving linear operator equations by operator factor- 
ization [8—14]. After the recursive Newton- Euler formulation [4,5] of manipulator dynamics is restated in 
an operator form, concepts developed in [6], which extend the classical signal filtering theory of [8-14] to 
mechanical systems, are used to achieve the operator factorization. These algorithms are developed in the 
context of classical mechanics, and thereby avoid the need to learn the nonstandard Spatial Dynamics of 
[2,3]. In this paper, the focus is on the open chain serial link case. The extension to the closed-chain case 
is reported in the companion paper [15]. 

A forward dynamics algorithm is first developed which computes and subtracts out the coriolis, cen- 
trifugal, gravity, and contact force bias terms, exactly as discussed above for the algorithms in [1,3]. After 
this step, as a consequence of an operator factorization and inversion of the manipulator mass matrix, 
joint-angle accelerations are obtained in O(N) iterations. This two-step algorithm is examined because 
it focuses on the key issues of mass matrix factorization and inversion. Alternative algorithms are then 
found by the use of certain operator identities. These include an algorithm which avoids the need for a 
preliminary bias computation and subtraction. 

The operator formulation of manipulator dynamics developed here has great conceptual and practical 
power. A conceptual strength of this approach is that abstract terms or expressions involved in manip- 
ulator dynamics and control, such as those which arise from a Lagrangian analysis, often have operator 
interpretations which make them implicitly equivalent to tip-to-base or base-to-tip recursions. For exam- 
ple, the manipulator mass matrix, X, will be shown to represent an unstated base-to-tip followed by a 
tip-to-base recursion. In fact, manipulator dynamics expressed in the Lagrangian formulation have an op- 
erator interpretation which results in exactly the Newton-Euler recursions, and the well-known equivalence 
of the Lagrangian and Newton-Euler formulations [16] is directly seen. 

Abstract analytical expressions, involving quantities such as the mass matrix X, can be manipulated 
with greater ease when viewed from the operator perspective. This is because the operator interpretation 
enhances the ability to manipulate such expressions by the use of operator identities which are given and 
derived in this paper. The abstract forms resulting from such manipulations can be interpreted as operator 
expressions which have equivalent recursive forms relating interlink forces and accelerations. This in turn 
can give insight into the physical meaning of the abstract expressions. 

The practical value of the operator algebra results from the ability to take the recursive equivalent of 
any operator-interpreted expression (which is coordinate-free) and immediately obtain an implementable 
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| recursive algorithm by projecting the recursion into appropriate coordinate frames attached to the links or 

j to an inertial reference point. 

j Although this paper focuses on obtaining forward dynamics algorithms, this by no means exhausts the 

applications of the operator approach. For example, [6,7] derive efficient iterative methods for obtaining 
the entire robot mass matrix and its inverse, as well as producing exact symbolic expressions for these 
| matrices. In [15,17], which develop forward dynamics algorithms for closed-chain systems, it is shown 

how with J the manipulator Jacobian, can be recursively derived. The inverse matrix, i.e., 

[JM -1 J T ] _1 , is exactly the operational space manipulator mass matrix of [18]. In a similar way, recursions 
for computing other terms involved in the operational space formulation of manipulator dynamics can be 
obtained. Additionally, it is anticipated that manipulator control laws under development [19] can be 
shown to have recursive implementations once an appropriate operator reformulation is performed. 

For clarity and brevity of expression, the algorithms of this paper are given in coordinate-free form. 
Of course, in actual applications the algorithm calculations can be performed in fixed link frames utilizing 
link-to-link transformations as is done in [1,4]. 

2. PROBLEM FORMULATION AND STATEMENT 

Consider a rigid AT-link serial manipulator as illustrated in Figs. 1.1 and 1.2, with the quantities 
defined in Table 1.1. The links and joints are numbered in an increasing order that goes from the tip of 
the system toward the base. Joint N is the last in the sequence, and it connects link IV to a base. In the 
body of the paper, the base is assumed to be immobile. The extension to a mobile base may be found 
in Appendix B. The base is referred to as “link N + 1”. Joint k in the sequence connects links k and 
k+1. The point O a can be selected to be any arbitrary point in link 1. Note that link and joint numbers 
increase toward the base of the system. This differs from the more common numbering approach in which 
the numbers increase toward the tip. 

This ordering allows one to think of sequentially moving from joint 1 to joint N as going “forward” 
and moving from joint N to joint 1 as going “backward” . In this paper, an algorithm which processes 
link data by iterating from k = 1 to k = N is then called “causal” while an algorithm which iterates from 
k = N to k = 1 is called “anticausal” . A complete tip- to-base causal iteration or a complete base-to-tip 
anti-causal iteration is called a sweep. 

The external environment is viewed as “link 0” , and the arm can contact the environment at point 
O 0 ■ Although 0(k) is defined to be rotational, the extension to the sliding joint case is simple and will be 
discussed later. 

Note that axis h(k ) is associated with angle 0(k) and both are associated with link k. This feature is 
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not true for the standard Denavit-Hartenberg formulation for numbering links, angle and axes discussed 
in [4]. This feature is true for the modified D-H formulation described in [2], and the scheme used in this 
paper can be considered to be that of [2] but with reversed link numbering. 

By standard classical kinematical and dynamical considerations [4,5], the equations relating link ve- 
locities and accelerations and the link force/torque balance equations are readily obtained resulting in the 
following “recursive Newton-Euler” (RNE) equations. The indicates time differentiation taken in an 
inertial frame. Also v(N + 1) = (9.8m/s 2 )e3 may be taken as a means to incorporate the effect of a uniform 
gravity loading on the links when determining link forces and moments, in which case v(k) is not the true 
link k linear acceleration. e 3 is defined as e 3 = (0,0, l) T . 

RECURSIVE NEWTON-EULER EQUATIONS. 


u/(N + 1) = 0; v(N + 1) = 0; u(N + 1) = 0; v(N + 1) = 0 or (9.8m/2)e 3 ; (2.1) 

for k = N, . . . , 1 loop 

cu(k) = oj(k + 1) + h{k)9{k) (2.2) 

v(fc) = t >(fc + 1) + u>(k + 1) x t[k + 1, A:) (2-3) 

w(Jfc) = u{k + 1) + h{k)6(k) + uj(k + 1) x h{k)0(k) (2.4) 

v(k) = v(k + 1) + 6j(k + 1) x t(k + 1, k) + u)(k + 1) x [w(A; + 1) x t{k + 1, &)] (2.5) 

v c (k) = i>(k ) + u>(k) X p(k) + u(k) X [<*>(&) x p(fc)] (2.6) 

end loop; 

a>(0)=u>(l); v(0) = v(l) + w(l) x £(1,0); w(0) = cO(l); 

v(0) = t)(l) + w(l) x £(l,0)+w(l) x [w(l) x£(l,0)] (2.7) 


F(0) = F ext] N(0) = N ext 

for k = 1 ,N loop; 

N c {k) = I c {k)w(k) + u{k) x I c (k) • oj(k)- 
F c (k) = m(k)v c (k) 

F(k) = F(k - 1) + F c (k) 


( 2 . 8 ) 

(2.9) 

( 2 . 10 ) 

( 2 . 11 ) 


N{k) = N(k - 1) + l(k , k - 1) x F(k - 1) + p(k) x F c (k) + N c (k) 

T{k) = h T {k)N{k) 


( 2 . 12 ) 

(2.13) 


end loop; 


The quantities referred to as “spatial velocity” V(k), “spatial force” f(k), “spatial acceleration” a(k), 
and “spatial inertia” M[k ) are now defined, (vy = v X y). 


y <*>- ($!)■• ■«-($})* m = 

M(k)-( m(k)p(k) \ 

\-m(k)p(k) m(k)I )• 


(2.14) 


(2.15) 


I denotes the identity operator. Note that the spatial quantities defined by (2.14) and (2.15) are not those 
given in [2,3], although they are quite similar. This distinction is quite significant because it implies that 
only the rules of ordinary matrix algebra are needed here. Thus the nonstandard Featherstone spatial 
algebra of [2] is not used in this paper. Define also 

*(* + l,t)=(' 't** 1 '*)); *’’(*)= (*£>). (2.16) 

With the above definitions (2.1)-(2.6) become 

V(N + 1) = 0 

^(fc) = <t> T (k + 1, k)V{k + 1) + H T {k)9{k) (2.17) 

and 

a(N + 1) = 0 

a(k) — <f> T (k + l,fc)a(A: + 1) + H T (k)9{k) + a(k) (218) 

where a(fc), the “bias acceleration”, is given by 


a{k) = ( 


c o(k + 1) x h{k)9(k) \ ( 

u(k + 1) x [w(fc + 1) x £(k + 1, fc)] / \ 


w(fc + l)xw(fc) \ . s 

u(k + 1) x [w(Jfc + 1) x i{k + 1, Jfc )] ) • ^ 


Note from (2.17) that <f> T (k,k — 1) is precisely the link k Jacobian operator which relates rates at point 
0(k) to rates at point 0(k — 1) [5]. 

Equations (2.8)— (2.13) can be recast as 

(JBMi •?)(%$) m 
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and 


(N c (k)\ = ( I‘(k) 0 \(u m\ + ( u,(k)xl c {k).u(k) \ 

\ F c (k) J \-m(k)p(k) m(k)I ) \ v(k) ) \m(k)uj(k) X [w(fc) X p(k)] ) 

Inserting (2.21) into (2.20) then results in 


( 2 . 21 ) 


f(k) = <f>(k, k - 1 )f{k - 1) + M{k)a{k ) + b{k) (2.22) 

/( 0) = / e xt; T(k) = iy(fc)/(fc) 


where 6(fc), the “bias spatial force”, is given by 

m = ( p' 23 ) 

1 ' \m(k)uj(k) X [uj(k) x p(k)\ J v ' 

To arrive at (2.22)-(2.23), the facts that I(k) = I c {k) - m(k)p 2 {k) and uj(k) X I c (k) ■ w(k) + m(k)p(k) X 
[w(jfc) X [w(Jfc) x p(Jfc)]] = w(fc) X I(k)uj(k) were used. Assuming, for now, no gravity loading, equations 
(2.17),(2.18)-(2.19), and (2.22)-(2.23) result in the following iterative equations. 


RECURSIVE SPATIAL DYNAMICS (RSD) . 




V(N+1) = 0; a{N + 1) = 0; 

(2.24) 

for k = N, . 

. . , 1 loop 

V{k) = <f> T {k + l,k)V{k + 1) + H T {k)6{k) 

(2.25) 



a(Jfc) = <f> T { k + 1, k)a{k + 1) + H T {k)6{k) + a(k) 

(2.26) 

end loop 


V(0) = ^ T (1,0)V(1); a(0) = <^ T (l,0)a(l) + a(0); 

(2.27) 



H 

II 

g 

(2.28) 

for k — 1 , . . 

. , N loop 

f{k) = <f>{k, k - l)f{k - 1) + M{k)a{k) + b{k) 

(2.29) 



T(k ) = H(k)f(k); 

(2.30) 


end loop; 

Note that V (0) and a(0) are the manipulator tip spatial velocity and acceleration and /( 0) is the tip spatial 
force acting on the external environment. Note also that a(k) = a\V(k + l),V(fc)] and b{k ) = 6[U(&)] so 
that at the k th iteration a(k) and b(k) can be computed from available quantities. 
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If a joint A; is a sliding joint rather than rotational, the RSD equations should be modified by taking 


= (*(*)) P' 31 > 

instead of (2.16) and taking 

= («(Jb + l) x [w(Jb+l) x£(Jfe + l,Jfc) + /i(Jfc)0(Jfc)] 

0 

u(k + 1) x [w(fc + 1) x C(k + 1, k) + v(fc)] 
instead of (2.19). Here 9(k) now denotes a sliding degree of freedom. Thus (2.14)-(2.16), (2.19), (2.23), 
(2.24)- (2.30), (2.31) and (2.32) completely describe the kinematics and dynamics of an arm with both 
sliding and rotational joints. 

Assume henceforth that [0(fc), 0(k)] are known. Given this knowledge, the forward dynamics problem 
is to obtain 9(k) from known inputs T(k) and the RSD equations (2.24)— (2.30). 


(2.32) 



3. PROBLEM STATEMENT IN OPERATOR FORM 

Define = 0(j) — 0(i ) to be the vector from O(t) to 0(j). Define also 

«*'•'■)= (o V)' M 

The matrix <f> T (i,j) is the Jacobian which relates the spatial velocity at point i to spatial velocity at point 
j. It is known [6] that <f>{i,j) obeys the “group properties” 

*(»,») = I; = *(j,t); <t>(i,k)</>(k,j) = <t>{i,j). (3.2) 

These are also referred to as the state transition operator properties [20]. Note in particular that 


Define 


k ) = *(», * - 1) • • • <f>{k + 1, A:). 


* = 


/ ^( 1 , 1 ) 0 
*( 2 , 1 ) *( 2 , 2 ) 


0 \ 


\HN, 1 ) 2 ) ... <f>(N,N)J 


H = diag[H(l),...,H(N)} 


(3.3) 


(3.4) 


(3.5) 
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M = dta<7[M(l), . . . ,M(N)] 


(3.6) 


B T = [4? { 1,0),0,...,0] 


(3.7) 


and 


Also, let 


£=[0,...,0,<£ T (A + 1,A)]. 


y = 

V'(l) 1 

<*> 

II 

' ‘ ‘ 1— * 

II 

T(l) \ 


{v{N)J 


{nun 



a(l) A 

«( 1) \ 

m ) 

( 6(1) 

a = 

: ; a = 

; ; / = 

• 

; 6= : 


[a(N)J 

[a(N)J 

fW) 

\b(N)J 


Note that the block diagonal of <f> is filled by the identity operator since i) = J. 


(3.8) 

(3.9) 
(3.10) 


From (2.25), 


or, using (3.1)-(3.9), 


V{k) = 'jT<t> T (i,k)H T (i)6(i) 

%—k 


v = <t> T H T e. 


(3.11) 


(3.12) 


Since (3.12) is just a restatement in “operator form” of (3.11), <f> T H T must be an anticausal operator. 
That <f> T H t is anticausal is equivalent to <f> T H T being upper block triangular. Actually, <f> T is anticausal 
(upper block triangular) while H (being block diagonal) is “memoryless”. This makes the product <f> T H T 
anticausal. Recall that (3.12) being anticausal just means that V is obtained from an outward iteration 
from the base to the tip. 

Since V(0) = ^ T (1,0)V(1), 

^(0) = J9 

J = B T <j> T H T . (3.13) 

J is nothing more than the “Jacobian Operator” which relates joint rates to the tip spatial velocity. J 
is seen to be an anticausal (t.e., outward sweeping) operator. In (3.13) the dependence of the Jacobian 
Operator J upon the interlink Jacobians <f> T {i,j ) is made explicit. 
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The entire manipulator dynamics cam be expressed in a variety of operator forms useful for solving 
the forward dynamics problem by operator factorization techniques. 

From (2.26), 

<*(*) = + <*(*)]• (3.14) 

i=k 

With (3.1)-(3.10), this becomes 

a = fH T 0 + <f> T a. (3.15) 

The accelerations a are seen to be the result of the joint accelerations H T 9 and the bias accelerations a 
propagated from the base to the tip of the manipulator under the influence of the anticausal operator of 
interlink Jacobians, f . 

From (2.29) and (2.30) 


k 

f( k ) = + J 0) + S U> i)^ 1 ) 0 )/^)] (3.16) 

1 = 1 

T(k) = H(k)f(k) 

or, with (3.15), 

f = <f>{Ma + b + Bf{0)) (3.17) 

T = Hf\ a = f H T 0 + fa. 

f is seen to be due to the causal propagation (due to the action of the lower block triangular operator <j > ) 
of the D’Alembert forces Ma, the bias forces b, and the tip forces /( 0), from the tip to the base. T is seen 
to be the memoryless projection of / onto the joint axis. From (2.27), <x(0) = B T f H T 0 + B T fa + a(0), 
and, since a(0) = JO + JO, then JO = B T fa + a(0). 

Equation (3.17) can also be written as 


T = H<f>[Ma + b + Bf( 0)] 
a = f H t 0 + fa 

where Ma are the D’Alembert forces. Alternatively, 


T = H<j>MfH T 6 + H<f>[Mfa + b + Bf{ 0)] 
= H4>[Mf{H T 0 + a) + b + B /(0)] 


(3.18) 


(3.19) 
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or 

T = Mi) + C + J T f{ 0) 

M = H<f>M<f> T H T (3.20) 

C = + b) 

J T = 

Eqs. (3.17)— (3.20) assume no uniform gravity loading. The effect of a uniform gravity loading on the 
manipulator is found by applying a pseudo-spatial acceleration to the base frame in the standard way [4], 
This results in the reassignment 

a^a + Eg, g T = (0 r ,9.8eJ) (3.21) 

where E is given by (3.8). 

Equations (3.17)-(3.20) give the operator formulations of the manipulator dynamics. In particular, 
the operator form (3.17) says that T is obtained by an anticausal operation on 0 and a, followed by a 
causal operation on a, b , and /( 0). This is, of course, precisely the recursive Newton- Euler algorithm of 
[4]. The fact that Eq. (3.20), which is of the form obtainable from a Lagrangian dynamical analysis, has 
an operator interpretation which results in the recursive Newton-Euler formulation reflects the well-known 
equivalence between Lagrangian dynamics and Newton-Euler dynamics [16]. 

The operator X will be called the “mass operator”. The mass operator is seen in (3.20) to have 
a natural causal-memoryless-anticausal factorization. Note that X cannot be inverted by inverting the 
individual factors since H<j> is nonsquare. In the next section, it will be shown that X has an alternative 
causal-memoryless-anticausal factorization for which the individual factors are invertible. 

Equation (3.20) can be written as 

MO = T (3.22) 

in which T' — T — T, with the “bias torques” T given by 

T = H<j>[M<f> T a + b+B /( 0)] = C + J T f{ 0). (3.23) 

Computing T recursively via the algorithm implicit in (3.23) allows one to work with the simpler system 
(3.22). Obtaining T from (3.23) is equivalent to using the RNE algorithm implicit in (3.17), but with 0 = 0. 
This approach to simplifying from (3.20) to (3.22) is the standard one and is used in [1—3]. A choice exists, 
then, to solve the forward dynamics problem by either solving (3.20) directly for 0, or by solving the simpler 
system (3.22) for which the bias torques have been removed. The second choice is considered in the next 
section, where the focus is on operator factorization and inversion techniques appropriate for inverting the 
mass operator. The algorithmic alternative represented by (3.20) is developed and presented in Section 5. 
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4. A FORWARD DYNAMICS ALGORITHM 
BY MASS MATRIX/OPERATOR FACTORIZATION 

Note that the operator At in the bias-free robot dynamics equations (3.22) is symmetric positive 
definite. This follows from (3.20). If such an operator can be modeled as the covariance of an output 
from a known, causal, and finite-dimensional linear system driven by white noise, then the operator can be 
factored and inverted efficiently by the use of standard techniques from filtering, detection, and estimation 
theory [8-14]. This factorization and inversion leads to a solution of (3.22). With A1 = H<t>M<f> T H T , such 
a model is immediately at hand. Indeed, taking 

Y = H<f>W (4.1) 

E{W) = 0; E(WW t ) = M, 

where Y T = [yf, . . . , yjy] and W T = [wj’, . . . , u$], results in E{Y ) = 0 and 

E{YY t ) = H<t>M(j> T H T = M. (4.2) 

Here, E(-) is the statistical expectation operator [14]. Note that (4.1) is just a succinct way of saying 

*( 0 ) = 0 ; 


for k = 1, . . . , N loop 


end loop; 


z(k) = <f>(k, k — l)z(fc — 1) + w(k)\ 
y{k) = H(k)z(k)] 


(4.3) 


where E[tn(fc)] = 0 and E[t«(fc)u/ r (fc)] = M(k). The operator formulation of (4.1) and the state space (or 
algorithmic) formulation of (4.3) are entirely equivalent. 

We have just seen that the model (4.1) results in the particular factorization At = H<t>M<f> T H T . In 
fact, there are an infinity of possible factorizations for A(, each one associated with a particular model. 
This is discussed in Chapter 9 of [14]. All such models for A1 are related and form an equivalence class. 
One member of this class, in particular, is viewed as canonical. This is referred to as the “innovations 
model” or the “innovations representation” . Under reasonably mild technical assumptions , which are met 
here, the innovations model for A1 is obtainable from any other available model for Al, such as the model 
(4.1). The factorization of A( associated with the innovations model is a primary goal of this section. 
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The quantities P(A:), D(k) and L{k) y k = 1, . . . , N, are now defined by the following iteration 

P(l) = M(l), D{ 1) = H(l)P(l)H T (l), L( 1) = $(2, 1)P{1)H T (1)D(1)-, (4.4) 

for k = 2, . . . , N — 1 loop 

P()t) = <£(Jk, A: - l)[P(Ar - 1) - P{k - 1 )H T (k - 1 )D~ 1 (k - l)H[k - l)P(k - 1 )}<f> T (k, k - 1) + Af (*); (4.5) 

D(k) = H(k)P(k)H T (k)] (4.6) 

L(k) = 4>{k + 1, k)P{k)H T {k)D-'{k)-, 

end loop; 

It can be shown that P(k) = P T (k ) > 0 for all k. Hence, the scalar D(k) is always nonzero, and 
D~ 1 (k) = 1.0/D(A:) is guaranteed to exist. 

Define t/>(i, j) for * > j by 

*l>(k,k) = I 

ip(k, k - 1) = 4>(k, k - 1) - L{k - l)H(k - 1) (4.7) 

#‘,j) = «/'(«', * — 1)0(* — 1, t — 2) - - * tp(j + 1 ,j)i i > j 

Also define the memoryless operators 

P = diag[P{ 1), . . . , P(N )\ ; D = diag[D( 1), . . . , D{N)}; L = diag[L( 1), . . . , £(#)] (4.8) 

and the causal operators 


$ = 


* = 


0 0 \ 

<f>[2,2) 0 0 

<K 3,2) <£(3,3) 0 0 

U(^,2) (f>(N,3) 4>(N, 4) ••• ^(I\r,AT) 0 / 

/ 0 °\ 

V>(2,2) 0 0 

V>(3,2) V>(3,3) 0 0 


\rp(N,2) xl>{N, 3) rP(N, 4) ••• t/>(I\T,A) 0 J 


(4.9) 


(4.10) 
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Note that $ of (4.9) is distinct from <f> of (3.4). Note also that: $ and V are strictly lower block- 
triangular (strictly causal); that D is symmetric positive definite; and that the block subdiagonals of 
and $ are filled by the identity operator, since ip(k,k) = tp(k,k) = I . 

It is now necessary to establish a few preliminary facts. Proof of the following Fact 1 is given in 
Appendix A. Alternative derivations can be found in [6], [7], [13] and [14]. Furthermore, the physical and 
statistical meanings of many of the quantities defined and used in this section and in Appendix A are 
discussed in [6,7]. 

FACT 4.1. An alternative factorization of M ~ H<j>M<f> T H T is 

M = {I + H$L)D{1 + H$L) t (4.11) 

where I + H&L is causal (lower triangular) and D is memoryless and invertible . 

Proof : See Appendix A. 

The model for X which results in (4.11) is the innovations representation and is given by the following 
causal operation on the “innovations” e T = [e T (l), . . . ,e T (A r )]: 

Y = ( I + H$L)e 

E{c) = 0; E{cc t ) = D (4.12) 

E{Y) = 0; E{YY t ) = M. 

Y = (I + H&L)e can be restated in equivalent state space (or algorithmic) form as 


for k = 1, . . . , N loop 


0 ) = 0 ; 6 ( 0 ) = 0 ; 


z(k) = <j>(k,k - 1 )z(k - 1) + L(k - l)e(fc - 1); (4.13) 

y(Ar) = tf(A:)z(fc) + e(A:); 

end loop; 

It is known that the inverse of the innovations representation is a Kalman filter, viewed as a whitening 
filter. This statement will be clarified after the statement of another needed fact. 
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FACT 4.2. The lower triangular operators I + H$L and I - H'hL are mutually reciprocal 


(I + H$L)~ X = 1- H'l/L. (4.14) 

Proof : See Appendix A or the comment below. 

The relationship e = (I + 5$L) _1 y = (J - H' J>L)y has the state space form (taking L(0)H{0) = 0 

2 ( 0 ) = 0 , 1 /( 0 ) = 0 ; 

for k = 1, . . . , N loop 

z(k) = [<f>(k, k - 1) - L{k - 1 )H(k - l)]z(A: - 1) + L{k - l)y{k - 1); (4.15) 

e(k) = -H{k)z{k) + y(fc); 

end loop; 

Equation (4.15) is the Kalman filter associated with the model (4.3), written as a whitening filter 
which produces the “white-noise” innovations sequence e(k). Hence, z(k) = E[z(fc)/y(l), . . . ,y(k - 1)], if 
w(k) is gaussian. Also note that z(fc) of (4.13) and (4.15) are identical, t.e., z(fc) of (4.13) is also a Kalman 
filter state. Comment: Note that since (4.15) follows from (4.13) by a simple rearrangement of terms, Fact 
4.2 is easily shown to be true. 

From Facts 4.1 and 4.2 immediately follows the key result of this paper. 

THEOREM 4.1. The operator X -1 can be factored as 

M~ x = (I - HVLfD-'V - HVL). (4.16) 

Theorem 1 and the bias-free robot dynamics (3.22) immediately imply the following forward dynamics 
algorithm. 

Algorithm FDl. 

T' = T - H<f>[M<t> T a + b + 5/(0)] 

9 = (J - 54'L) r 5" 1 (J - HVL)T'. 
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(4.17) 

(4.18) 



As discussed in the last section, (4.17) involves the O(N) recursive Newton-Euler algorithm for 0 = 0. 
Equation (4.18) is given by a causal (tip-to-base) sweep to produce the normalized innovations v = D~ 1 e 
with e = (I — H'ftL)T’, followed by an anticausal (base-to-tip) sweep to produce the joint accelerations 6. 
Since (4.17) additionally involves an anticausal sweep followed by a caused sweep, we call algorithm FD 1 
a “4-sweep” algorithm. 

In state-space (algorithmic) form, (4.18) becomes 
i) Causal Filtering of Bias Free Joint Moments: 

z(0) = 0, T'(0) = 0; (4.19) 


for k = 1, . . . , N loop 

z(k) = k - 1) - L{k - 1 )H{k - l)]z(A: - 1) + L(k - 1 )T'(k - 1); (4.20) 

e(k) = T{k) - H(k)z(k); 
v(k) = D* 1 (k)e(k)] 

end loop; 

ii) Anticausal Smoothing of Weighted Residuals: 

X[N + 1) = 0; (4.21) 


for k = N , . . . , 1 loop 

A(Jfc) = ^(ife + 1, Jfc) - L{k)H (k)] T X(k + 1) + H T {k)u{k)] (4.22) 

0(k) — i/(k) — L T (k)X[k + 1); 

end loop; 

Note that the algorithm (4.19)— (4.22) obviously has an operations count which is O(N). What has 
not been made explicit in the above algorithm is the need to recursively construct V,a,b,P, D, and L. 
For notational simplicity the need for these recursions is usually not stated, it being understood that they 
will be performed during appropriate sweeps of the algorithm. For example, V and a can be constructed 
during the l* 4 (anticausal) sweep of (4.17) indicated by M<f> T a, while b,P,D, and L can be constructed 
during the 2 nd (causal) sweep of (4.17) indicated by H<f>\'"\. 
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5. ADDITIONAL FORWARD DYNAMICS ALGORITHMS 
FROM THE USE OF OPERATOR IDENTITIES 


Various alternative formulations of the forward dynamics algorithms can be obtained by the use of 
operator identities which are derived in Appendix A. We will first show how a slight modification of 
the 4-Sweep algorithm of the last section leads to a 4-Sweep algorithm which produces the link spatial 
accelerations a and the tip spatial acceleration cx(0). Define 


0(1,1) 



0 A 

0(2,1) 

0(2,2) 


0 

0(3,1) 

0(3,2) 

0(3,3) 

0 

0(AU) 

0(N,2) 

0(JV,3) . 

.. 0(N,N); 


/° 

0> 1 



JO 0 


S = 


0 I 


0 


VO ••• J 0) 


and note that 


* = 0S. 


(5.1) 


(5.2) 


(5.3) 


IDENTITY 5.1. For <f> given by (3.4), 'i' given by (4.10), and t/> given by (5.1), 

(J - H*L)H<I> = Hil>. 


(5.4) 


Proofs : The proof may be found following the set of identities A.4 given in Appendix A. However, the 
following alternative proof is more transparent: A state-space representation of X = H<f>W and Y = 
(J - H*L)X = {I- H*L)H<f>W is 

z(k) = <t>(k,k — l)z(fc — 1) + w(k)] x(k) = H(k)z(k)\ 

f(Jfc) = [^(fc,Jk - 1) - L{k - l)H{k - 1 )}${k - 1) + L{k - l)x{k - 1); 

y(k) = -H(k)$(k) + x(ky, 

yielding 

z(k) - ${k) = [4>{k,k- 1) - L(k - l)H{k - 1 )}[z{k - 1) - f(fc - 1)] + w(fc); 

y(A:) = jr(A:)[z(A:)- ? (A:)]; 
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or 


Y = HxpW. 


This identity is now used to obtain the following 4-Sweep algorithm for link spatial accelerations and joint 
accelerations. 

ALGORITHM FD2. 


T =T - H<f>[M<f> T a + 6 + B /(0)] ; 
a = - H*L)T' + <j> T a\ 

a(0) = B t a + a(0); 

Pjroo£ : From (3.15), a = <j> T H T 0 + <f> T a. With (4.18), this gives 

a = <f> T H T (I - H^L) T D~ l {I - HVL)T' + <f> T a 
= ^ T ff T H _1 (J - H<flL)T' + <f> T a. 

The last step follows from (5.4). Eq. (5.7) follows from (3.7) and (2.27). 

Note that A of (4.22) obeys A = ip T H T u, v = D~ 1 e, e = (J — H^/L)T'. This gives A = 
tP T H T D~ 1 (I - H*L)T'. Hence, with (4.18) and (5.3), 9 = (J - L T S T ij> t H T )v = v - L T S T A. Also, 

from (5.6), a = A + <f> T a. This means that a slight modification of the forward dynamics algorithm 
(4.19)— (4.22) allows the computation of the manipulator tip acceleration, a(0), in addition to 0. This 
occurs by changing (4.21) and (4.22) to 

A {N + 1) = 0; f (AT + 1) = 0 (4.216) 

for k = N, . . . , 1 loop 

A(Jb) = [<f>(k + l,k)~ L{k)H{k)] T X(k + 1) + H T (k)v(k)-, (4.226) 

0(k) = u(k) - L t ( k)X(k + 1); 

?(*0 = <t> T (k + 1, k)$(k + 1) + a(k)\ 


(5.5) 

(5.6) 

(5.7) 
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a(*) = A(fc) + f(fc); 


end loop; 

a(0) = <f> T { 1, 0)a(l) + a(0). (4.23) 

Algorithms FD1 and FD2 are both “4-Sweep Algorithms”, 2 sweeps being required to compute the 
biases, followed by 2 sweeps to complete the computation of joint rates or spatial accelerations. The next 
two algorithms are 3- Sweep Algorithms. 

ALGORITHM FD3. 

? = 4? a) 

e = T- Hxl>[SLT + Mf + b + 5/(0)]; 

9 = (J - HVLfD-'e-, 

Proof : From (3.19) 

H<t>M<t> T H T 6 = T — H(j>[M f + 6 + 5/(0)]. 

Note that, see (5.3), 

(I - H*L)T = (J - H\l>SL)T = T- HipSLT. 

Also note (5.4). With Theorem 4.1 the above yields 

9= (I - HVty-'D-'lT-HtpiSLT + Ms + b + BfiO)]. 

Algorithm FDZ requires anticausal-causal-anticausal sweeps to obtain 0 as indicated respectively by (5.8), 
(5.9) and (5.10). In FDZ, V and a can be computed during the anticausal sweep (5.8), and b,P,D, and L 
during the causal sweep (5.9). In the same way that FD2 was derived from FD 1, algorithm FDZ can be 
modified to compute link and tip spatial accelerations although this is not done here. When obtaining the 
recursive forms of algorithms expressed operationally, such as the algorithms (5.8)- (5.10), the identities 
A.l given in Appendix A are useful. 

Note that Eq. (5.8) reflects a need to have a preliminary step to compute the bias accelerations a. The 
last algorithm given in this paper removes this requirement, although the need for a preliminary anticausal 
sweep for the purposes of computing V remains. We first define 

ij) = V’ — / (5-ii) 

and 

G = PH t D~ 1 . (5.12) 

We have 


(5.8) 

(5.9) 
(5.10) 
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ALGORITHM FD4. 


V = B T <f> T H T 6 ; (5.13) 

v = I - H*L)T - D~ 1 HrpPa - D~ x H\p\b + Bf{ 0)]; (5.14) 

$ = (J - H*L) t v - L r * r ( J - Gff) r a - G r a; (5.15) 


Proof: See Appendix A. 

Writing (5.14) and (5.15) as 

z = VLT + $Pa + # + B/( 0)); 
e = T — Hz\ 

u = D~ 1 e-, 

X = rf> T H T v + xl> T {I - GH) T a ; 

£ = v - L t S t X - G T a; 

shows that (5.14) and (5.15) can be implemented as 

z(0) = /(0); P(0) = 0; T( 0) = 0; 

for fc = 1, . . . , JV loop 

z(fc) = V>(it, * - 1 )z{k - 1) + L{k - l)T(k - 1) + il>{k, k - l)P{k - l)a(fc - 1) + 6(fc); (5.16) 

e(Jfe) = T(k) - H(k)z(k)\ 
i/[k) = D~ 1 (k)e(k); 

end loop; 

for k = N, . . . , 1 loop 

A(jfc) = rJ, T (k + l,k)X(k + 1) + H T {k)u{k) + [I - G(k)H(k)] T a(k); (5.17) 

8{k) = v(k) - L T (k)X(k + 1) — G T (k)a(k)\ 

end loop; 
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The 3-Sweep algorithm (5.16)-(5.17) can be found in [6], where, viewing the RSD equations as de- 
scribing a two-point boundary-value problem, it was derived by applying what [6] refers to as the “sweep” 
method to solve for 0(Jc). 

It is possible to apply the tools of this paper to obtain a “2-Sweep” forward dynamics algorithm, 
avoiding the sweep contained in (5.8), although at the expense of much greater algorithmic complexity. 
The development is somewhat lengthy, however, and will not be given here. 

6. CONCLUSION AND DISCUSSION 

This paper has shown the power of a linear operator approach to solve the manipulator forward 
dynamics problem. This approach is based on techniques for operator factorization and inversion that 
were originally developed in the context of filtering and estimation theory and that have recently been 
extended to the analysis of multibody mechanisms [6,7]. Other results of [6,7] not discussed here include 
the derivation of efficient iterative methods for obtaining the entire robot mass matrix and its inverse, as 
well as the derivation of exact symbolic expressions for these quantities. 

As a demonstration of the capabilities of this approach, several O(N) iterative algorithms for com- 
puting the forward dynamics of an iV-link rigid manipulator were developed. The first algorithm derived 
in this paper requires that the operator form of the robot dynamics be first placed in a bias-free form 
(3.21). Alternative algorithms which relax this requirement were then developed by using various operator 
identities. The ability to obtain alternative forward dynamics algorithms by the use of operator identities 
potentially allows for algorithmic modifications leading to greater computational efficiencies. 

In the companion paper [15], efficient iterative algorithms for solving the forward dynamics of a closed- 
chain system (comprised of several arms grasping a commonly held object) are given which further show 
the power of the approach presented here. 
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APPENDIX A: OPERATOR IDENTITIES 


In this appendix, Facts 4.1, 4.2 and Algorithm FDA are derived following the development of various 
useful and important operator identities. The derivations given here are “mechanical” and do not depend 
upon the insights available from the theory given in [8—14]. These insights greatly facilitate the under- 
standing of the algorithms developed in this paper, and the results proven in this appendix can also be 
obtained by use of the methods of [13] and Chapter 9 of [14]. See also [6,7]. 

Recall that <p is given by (3.4), $ by (4.9), ip by (5.1), ¥ by (4.10), ip, by (5.11), and S by (5.2). Also 
recall that P, D, and L are given by (4.4)— (4.8) and G by (5.12), giving 


D = HPH t 

(a.l) 

G = PH t D~ 1 

(a.2) 

We also define 


4> = <p- I 

(a.3) 

and 


A^ = diag[<p(2,l),...,<P(N + l,N)} 

(o.4) 

= diag[ip{ 2, 1), . . . , ip(N + 1, N)] 

(a.5) 

Our first block of identities is given by 


IDENTITIES A. 1. 


$ = <ps 

(a.6) 

V = ipS 

(o.7) 

A>A^, = ip 

(a.8) 

A/ 

= <P 

(o.9) 

ip- 1 = I - SAj, 

(o.lO) 

ip' 1 = I - SA* 

(oH) 

L = A+G 

(a.12) 

4>G = S>L 

(a. 13) 


Proof : Equations (a.6)-(a.9) and (a.12) are straightforward. Equations (a.10) and (a.ll) follow from 
(a.6)— (a.9) since ip = ip — I and <p = <p — I . Equation (a.13) follows from (a.12) and (a.9). 

A second block of identities is given by 
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IDENTITIES A. 2. 


<f> = V»(I + (a. 14) 

V- = (J - *£#)<£ (a. 15) 

$ = J + LH$) (a. 16) 

= (J - *Ltf)$ (a. 17) 


Proof: Equations (a.16) and (a.17) follow from (a.14) and (a.15) from postmultiplying by S and using 
(a.6) and (a.7). Equations (a.14) and (a.15) are both equivalent to 

<f> - tf> = *LH<t> = V> SLH<f> (o.l8) 

To prove (a.18) consider T(k,m) defined by 

h-1 

T(k, m) = \rp{k, i + l)^(* + 1, m) - xp(k,i)<f>(i,m)] for k > m (a. 19) 

i = m 

with T(k, m) = 0 for k < m. 

We have that 


T(k y m ) = t p(k,k)<f>(k,m) - xp(k,m)<f>(m i m) — </>(k,m) - ip(k,m) 


On the other hand 


k—i 


T{k,m ) = + l)<^(t' + l,m) - V'(* ) t + l)V'(« + l,t)^(«,m)] 

i—m 

k-1 

= YL + 1 )[^(* + i > m ) - w* + i.‘W» m )] 

i=m 

k - 1 

= + 1 )[^(* + i > m ) - w* + 1,0 - i (0 fl ’(0]^ , (*, m )] 


or 


k-i 


T[k, m) = ^ tp(k,i + l)L{i)H(x)<f>{i,m) 

i=m 

Equations (a. 20) and (a.21) yield 

k - 1 

<t>(k,m ) - tl>(k,m) = ^ ip(k,i + m) 

i—m 

for k > m, thereby proving (a. 18). 


(a. 20) 


(a.2l) 


A third block of useful identities is given by 
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IDENTITIES A. 3. 


= ( I + $IF)V> 

(a.22) 

xf> = <t>{I- SLHxp) 

(a.23) 

$ = (J + $£#)* 

(a.24) 

= $(J - £17 *) 

(a.25) 

= #£#> 

(a.26) 

$£17* = *£17$ 

(a. 27) 


Proof: Equation (a.24) and (a.25) follow from (a.22) and (a.23) from post-multiplying by S and using 
(a.6) and (a.7). Equation (a.26) follows from (a.22) and (a.14). Equation (a.27) follows from (a.26) by 
postmultiplying by S. Equations (a.22) and (a.23) are both equivalent to 


<f>-ip = QLHip = <j>SLHip 
To prove (a.28) consider T'(k,m ) defined by 

fc-i 

T'(k,m ) = + IjV’C* + l,m) — <f>(k,i)xp(i,m)] for k > m 

i=m 

with T'{k, m) = 0 for k < m. 

We have 

T'(k,m) = <f>(k,k)ip(k,m) - <f>(k,m)rp(m,m) = rp(k,m) - <f>(k,m ) 

On the other hand, 

k- 1 

T‘{k,m) = J^[^(fc,i + 1 )ip(i + l,m) - <p{k,i + 1)<£(i + l,t)t/>(«,m)] 

i—m 
k - 1 

= 52 + 1 > m ) - + !» *)#',"»)] 

t — 171. 

k~ 1 

= 52 ^(*>* + OM* + 1 >‘) - M* + MOM*,™) 

i—m 

or 

k- 1 

T'(k,m) = - 52 + l)L(i)£T(») rp{i,m). 


(a. 28) 


(a. 29) 


(a. 30) 


(a. 31) 
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Equations (a.30) and (a.31) yield 


k-i 

<f>{k,m) - ^ <f>{k,i + l)L(i)ff(i)i/>(i,m) 

i=m 

for k > m, thereby proving (a.28). 


Yet another block of useful identities is given by 

IDENTITIES A. 4. 

H<f> = ( I + H<PL)Hxl> (a. 32) 

H$ = ( J + H$L)H 9 (a. 33) 

X* = H$(I - LHV) (a.34) 

Hij> = (J - H*L)H<i> (a.35) 

H* = (I - H*L)H$ (a. 36) 

H$ = H*{I + LH$) (a.37) 

HQLH* = HVLHQ (a. 38) 

QL = *L{I + HQL) (a. 39) 

*L = ( I - *LH)<tlL (a.40) 

*L = $L(J - H'ifL) (a.41) 

= ( I + $LH)*L (a. 42) 

VLHQL = 9LHVL (a.43) 


Prop/ : Equations (a.32)-(a.38) follow from (a.22), (a.24), (a.25), (a. 15), (a.16), (a.17) and (a.27) by premul- 
tiplication by H. Equation (a.39)-(a.42) follow from (a.16), (a.17), (a.25) and (a.24) by postmultiplication 
by L. Equation (a.43) follows from (a.40) and (a.41). 

LEMMA A.l. For r = dioy[r(l), . . . ,r(i\T)] where 

r(0) = 0 

r(k + 1) = if>(k + 1 } k)r(k)<f> T (k + 1, k) + M(k + 1) (a.44) 
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we have 


rj>M<f> T — r + tpr + r<f > T . (a. 45) 

Proof : It is an exercise in algebra to show that for R = 0Af0 T , R(k, k) — r(fc) where 

r ( k ) = 

3=1 

obeys (a.44) and 

f?(Jfc,/) = rp{k,l)r{l) for k > l and R{k y l) = r{k)<f> T (l y k) for k < l. 

To show this, exploit the fact that M is block diagonal and 0 and <f> are both causal (lower block triangular) 
with elements which obey the semi-group properties 0(j, A:) = tp(j y l)xp(l } k) } k) = k) y and 

j) = 0(i,i) = /. Thus R = r + 0r + r0 T . 

COROLLARY A. 1 . : LEMMA A. I holds for 0 = <f>. 

Proof : The only restrictions on 0 and <f> are that they be causal with elements which obey the semi-group 
properties. 

/v_ 

Since ip is strictly causal (strictly lower block triangular), <fr is strictly anticausal (strictly upper block 
triangular), and r is memory less (block diagonal), LEMMA A.l is a statement about how the operator 
ipM<f> T can be decomposed into the sum of a memoryless term, a strictly causal term, and a strictly 
anti-causal term. 

IDENTITIES A. 5. 

tf>M</> T = P + rpP + P$ T (a.46) 

tPMip T = P + $P + P$ T . (a. 47) 

Proofj P = d»ag[P(l), . . . , P(N)} with P{k) given by (See Equations (4.4)-(4.6)) 

P(0) = 0; 

P{k + 1) = <j>{k + l,k)[P(k) - P(k)H T {k)D~ 1 {k)H(k)P(k)}<f> T (k + 1, k) + M(k+ 1) 

= ^(Jfc + 1, Jfc)[J - G(k)H(k)]P(k)<t> T (k + 1, fc) + M(fc + 1) 

= + 1, k) - L{k)H{k)]P{k)<j> T (k + 1, k) + M(k + 1) 
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= tP(k + 1, k)P(k)<f> T (k + 1, k) + M{k + 1). 


Equation (a.46) now follows from LEMMA A.l. Now note that 

[I - G(k)H(k)]P(k)H T (k)G T (k) = [P(k)H T {k) - P(jfc)ff T (fc)£)- 1 (fc)H(Jk)P(A:)ff T (Jk)]G 3 ’(fc) 

= [P(fc)P T (fc) - P{k)H T {k)]G T (k) = 0 

Thus 

P(fc + 1) = <f>{k + 1, k)[ I - G(k)H{k)]P{k)[ I - G{k)H{k)] T <f> T {k + 1 ,k) + M(k + 1) 

= xf>(k + l,k)P(k)xl> T {k + 1, k) + M(k + 1) 
and (a.47) follows from Corollary Al. 

IDENTITY A. 6. 

<f>M<f> T = P + $P+ P% T + 4>PH T D~ l HP4> T . (a. 48) 

Proofs : P = diag[P{ 1), . . . , P(7V)] with P{k ) given by 

P(0) = 0; 

P(k + 1) = 4>(k + 1, Ar)[P(Jfc) - P{k)H T {k)D~ 1 {k)H{k)P(k)]<l> T {k + 1, it) + M{k + 1) 

= 4>{k + 1, k)P(k)<l> T [k + 1, k) + x (k + 1); (a.49) 

where 

x(k + 1) = M{k + 1) — 0(k + 1); 

Q(k-hl) = 4>(k + l,k)P{k)H T {k)D~ 1 {k)H(k)P(k)<l> T {k+l,k). 

Define x = dta^[7r(l), . . . , tt(N)] and © = dia</[0(l), . . . , 0(N)]. Then x = M — 0. From (a.49) and 
Corollary (A.l), 

tf>x(f> T = P + 4>P + P4 > t 
or 

4>M4> t = p + $p+ P4? + <t>e<t> T . 

Algebraic manipulations show that 

<t>e<f> T = $PH T D* 1 HPj> T 
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proving our result. 


IDENTITY A. 7. 


(J - H*L) T D~ 1 Hil>M<t> T 

= (I- hvl) t d~ 1 h$p+ L t * t (I - GH) t + G t . 


(a. 50) 


j Proof : 

(7 - H'f/L) T D~ 1 H(rpM<t> T ) 

= (7 - H'ifL) T D~ 1 H(P + $P+ Pj?) from (a.45) 

= ( J - HVLfD-'H^P + (J - H't/L) T D~ 1 HP + (7 - H* L) T D~ x H Pj> T . 

Comparing the last expression to (a. 50), we see that (a.50) is true provided that 

(7 - hvl) t d- 1 hp + (I - HVL) t D- 1 HP4> t 

= -GH) t + G t ( a . 51) 

We have that 

(J - HVLfD-'HP + (7 - H'i!L) T D- 1 HP$ T 
= (7 - H*L) t G t + (7 - H*L) t G t $ t 
= (J - H*L) t G t + (7 - H*L) t L t $ t from (a.13) 

= (7 - ir*L) r G r + [$(7 - LH*)L] t 
= (7 — H^>L) t G t + from (a.25) 

= Z/ r * r (7-Gff) r + G r . 

This proves (a.51) and hence (a.50). 

PROOF OF FACT 4.1. 

With (a.13), it is enough to show that 

H<f>M<f> T H t = (7 + H4>G)D{I + H<j>G) T where G = PH T D~ 1 . 

We have that 

(7 + H4>G)D{I + H$G) T 
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= HPH t + H4>PH t + HP4> t H t + HtPH T D~ 1 HP4> T H T 
= H{P + $P + P4> t + $PH T D~ 1 HPt T )H T 
= H<f>M<f> T H T 

where the last step follows from (a.48). 

PROOF OF FACT 4.2. 

( J + H$L)(I - HVL) = I + H{$ -9- *LH$)L = I from (&.16). 

PROOF OF ALGORITHM FD4. 

From (3.19), 

H<f>M<l> T H T 0 = T- H<f>[M<f> T a + b+ Bf{ 0)] 

Theorem 4.1 and (a.35) are used to obtain 

0 = (I - H*L) t D-'{ 1 - H*L)T - (I - H*L) T D~ 1 Htl>[b + Bf{ 0)] 

-(J - HVLfD-'HtpM^a. 

Note that ALGORITHM FD4 can be written as 

0 = (J - H*L) T D~ l {I - H*L)T - (I - H*L) T D- 1 Hxl>\b + Bf( 0)] 

- [( J - H^fL) T D~ 1 HxfP + L t * t (I - GHf + G T ]a. 

The proof follows from (a.50). 
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APPENDIX B: EXTENSION TO A MOBILE BASE 


The extension here is to the case where the base is completely free to move throughout Euclidean 
3-space. In this case, the natural configuration space for describing the translation and orientation of the 
base is R 3 x 50(3), where 50(3) is the rotation group [21,22]. This results in no loss of generality as the 
procedure shown here for incorporating configuration, velocity, and acceleration parameters appropriate 
for describing base behavior into the manipulator equations has an obvious analogue for a choice of config- 
uration space other than R 3 x 50(3). In any event, every other possible configuration space for the base 
must be a submanifold of R 3 x 50(3) (e.g., R? for a nonrotating base constrained to the plane) and an 
appropriate restriction of base configurations, velocities, accelerations and forces in the general solution 
given here will result in the solution for any possible base configuration space. A restriction of R 3 x 50(3) 
to 50(3) is particularly easy and interesting as it is equivalent to making the base a 3 degrees-of-freedom 
(dof) spherical (ball-in-socket) joint, enabling the arm to have the anthropomorphic quality of a 3 dof 
spherical “shoulder-like” base. In fact, the procedure shown here for admitting 50(3) as a configuration 
space for the base can be used to model any arm joint as a 3-dof spherical joint. 

The first step is to now rename link N to be the base, whereas link N + 1 will now be called the 
“station” . In terms of the notation used in the paper, the arm would be an IV-dof mechanism, only now 
there must be added an extra 5 degrees of freedom, since R 3 x 50(3) is a 6-dimensional manifold. For 
now, however, continue to think of the base link N as having a 1 dof joint. 

The configuration parameter for joint N has been taken to be 0(N) € R 1 with a velocity 9(N) = dO/dt. 
There is a need to extend 9{N) to be a vector parameterization of base location and orientation (i.e., of 
R 3 x 50(3)), and to allow 0(N) to have an associated velocity which is not 0(N). For this reason, define 
W (k) = 6{k). With this change, the manipulator equations (3.20) become 

r = mw + c + J T f{o). (6.i) 

Equivalently, the RSD equations (2.24)-(2.30) hold with 6(k) replaced by W(k). The dynamical behavior 
of link N is given by the modified RSD equations for k — N. The velocity of the base link N is found from 
(2.25) to be 

V(JV) = <f> T (N + 1, N)V{N + 1) + H t (N)W{N). 

Take 0(N) to be any point on the base link N and take the station velocity V(N + 1) = 0. This gives 

V(N) = H t (N)W{N). 
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Now the appropriate velocity for describing the base link N is precisely W (N) = V(N), since V(N) = 
co/[w(JV’),t>(iV r )], where v(N) = £(N + 1 ,N), and w(iST) gives the angular rate of change of the attitude of 
the base with respect to the station [21,23]. Thus 

F(W) = H t (N)W{N) 

H t {N) = I e R 6x6 . (6. 2) 


Appropriate configuration parameters for the base link N must be chosen to describe the base in 
R 3 x 50(3). Take 0(N) € R 7 , where 0(N) = col[q,t(N + 1, JV)] with £{N + 1, JV) € R 3 being the base 
location with respect to the station. The Euler parameters (unit quaternions) q € i? 4 give the attitude of 
the base with respect to the station [21,23]. The Euler parameters are a globally nonsingular representation 
of 50(3), unlike the Euler angles which can become singular. The relationship between q and w(iV) is 
given by 

q = A (q)u(N) 

u = ir(q)q (6.3) 

where ft -1 (g) = jt (q), and A(q) are always nonsingular and can be found in [21-23]. This gives 

9(N) = Cl[6(N)]W(N) 

n[*Wl-( A ^ J). (M 

With 0 having elements 0(1), . . . ,0(N — 1) € R 1 , and 0(N) € R 7 , then 0 € R N+6 , where N is the number of 
links and iV + 5 is the total number of arm degrees of freedom. Now define H[0(fc)] = 1 for k = 1 , . . . , N - 1 , 
n[0(N)} by (b.4), and C1 = d»'ag[fi(A:)]. Then, 

0 = CIW. (6.5) 

Differentiation of (b.2) gives 

a(N) = H t (N)W(N) s W{N ), (6.6) 

with the station velocity a(N + 1) = 0, since gravity is being ignored. Eq. (b.6) shows that now 

a(N) = 0. (6.7) 

Eq. (2.29) for k = N is unchanged, but (2.30) is now 

T(k) = H(k)f(k) = f(k)eR 6 . (6.8) 
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The extension is now complete and (b.l)-(b.8) succinctly result in 

T = MW + C + J T f{ 0 ) 


0 = n W. ( 6 . 9 ) 

The operator forms and interpretations of .M, C, and J T still hold and in particular 

W = - C - J r /(0)] (6.10) 

can be obtained recursively by the operator factorization and inversion given in this paper. The only 
change is in the dimensions of the operators. All such changes are conformal and hence harmless. 


33 


Table 1.1 SYMBOL DEFINITIONS 


h(k) = unit vector along joint axis k 

6(k) = angle of link k with respect to link k + 1 about joint axis k 

0(k) — fixed point on joint axis k , which can be viewed as the origin of a frame fixed in link k 
l(k y k — 1) = vector from O(fc) to 0(k — 1) 

E(Jfc) = constraint force on link k at point 0(k) of joint k 

N(k) = constraint moment on link k at joint k 

F c (k) = net force on link k at link k mass center 

CM(k) = mass center of link k 

p(Ar) = vector from 0(k) to CM(k) 

t ?(k) = velocity of link k at point 0(k) of joint k 

u;(fc) = angular velocity of link k 

v c (k) = velocity of link k at link k mass center 

m(fc) = mass of link k 

I c (k) = inertia tensor of link k at point CM(k) 

I(k) = inertia tensor of link k at point 0(k) 

T(k ) = actuated torque at joint k 
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LINK N 


BASE (LINK N ♦ 1) 


Fig. 1.1 iV-Link Serial Manipulator 


h 


k-1 



Fig. 1.2 Relationship of Defined Quantities to Link k 
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Part H 

CLOSED-CHAIN DYNAMICS FOR MULTIPLE 
ROBOT ARMS MOVING A COMMON OBJECT 



ABSTRACT 


The forward dynamics problem for a closed-chain system comprised of t manipulators grasping a 
commonly held object is analyzed and solved. It is shown that this solution can be implemented either 
by using analytical expressions for the dynamics of each free arm, or by using any available open-chain 
forward dynamics algorithm which can be applied to each free arm. In particular, a recursive solution is 
presented based on an operator interpretation of the solution combined with an operator factorization and 
inversion of arm mass matrices. The resulting algorithm is shown to be but one of many possible recursive 
algorithms derivable from the operator approach applied to the analytical expression for the solution of 
the closed-chain forward dynamics problem. These algorithms require O(n) + 0(£ 3 ) operations, where 
n is the total number of manipulator links in the closed chain system. The factorization approach for 
the closed-chain system can be built up modularly from the forward dynamics solution for each arm of 
the open-chain system. In the main body of the paper, the grasps are taken to be rigid attachments, in 
the sense that there are no contact degrees of freedom left unconstrained by the grasps. In an appendix, 
nonrigid grasps for which there are a prescribed number of unconstrained contact degrees of freedom are 
considered. 


1. INTRODUCTION 

The ability to simulate dynamical systems comprised of closed-chains of interconnected rigid links 
is important for understanding issues such as bipedal or multipedal motion, or multiple arm cooperative 
grasp and manipulation of objects. The ability to efficiently obtain joint accelerations from known driving 
moments — to solve the forward dynamics problem — is an important step toward making such simulations 
possible at a reasonable computational cost. The solution of the forward dynamics problem can also provide 
a thorough mathematical and physical understanding of the way multiple arms behave dynamically in 
response to applied moments. This understanding is essential for designing effective control algorithms 
and for searching for the sources of anomalous behavior when it occurs. 

Several recursive approaches to the closed-chain forward dynamics problem have been proposed in [1- 
3] and [4-6]. Recursive approaches have the feature that they can be implemented having knowledge only 
of individual link mass and geometric properties, and of the nature of the link interconnections (joints). 
The need to have analytical expressions describing the complete manipulator dynamics is avoided. This 
makes it relatively easy to retarget the forward dynamics algorithms, and the corresponding software 
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implementation, from any given arm to another. 


In [1,2], a closed-chain forward dynamics algorithm is given based on the open-chain forward dynamics 
algorithms of [3], which utilize repeated applications of the recursive Newton-Euler algorithm [7,8]. The 
closed-chain algorithm of [1,2] is of complexity 0(t 3 N 3 ) = 0(n 3 ), where £ gives the number of arms which 
grasp a commonly held object and the total number of manipulator links is n = IN, assuming that each 
arm has N links ( N degrees of freedom). £ is also proportioned to the number of independent closed loops 
of the system considered in [1,2]. (In fact, the number of such loops is just £ — 1.) A strong point of the 
algorithm [1,2] is its conceptual simplicity. 

In [4], a solution to the closed-chain forward dynamics problem is outlined. This solution is of com- 
plexity 0(n) + 0(£ 3 ), where n is the total number of links in a closed-chain mechanical system and £ is the 
total number of independent loops. The work of [7] builds on [4] to obtain an iterative algorithm of com- 
plexity no greater than 0(n) + 0(£ 2 ) and, for some special cases, at times as low as O(n) +0(£ logt). The 
algorithms in [4,5] are developed using the nonstandard Featherstone Spatial Calculus (which is based on 
Motor and Screw Calculus). The need to learn the Featherstone Spatial Calculus makes these algorithms 
less accessible than algorithms such as those in [1,2] which are based on the classical (Gibbsian) vector 
calculus. 

In this paper, forward dynamics algorithms for a closed-chain system formed by several manipulators 
holding a common object are derived. An approach is taken which, before specializing to algorithms 
based on the operator approach and iterative techniques of [6,9-12], potentially allows any recursive open- 
chain forward dynamics algorithm to be used - e.g., those of [3] or [4]. In fact, the closed-chain forward 
dynamics problem is expressed in a form which allows for the use of closed-form analytical expressions for 
the dynamics of each arm, if and when these expressions are available. 

The recursive algorithms given here are of complexity O(n) + 0(£ 3 ), where £ is the number of arms 
and n is the total number of arm links in the system. These algorithms axe based on the spatial dynamics 
of [6,10], and on the operator formulation of robot dynamics and the operator mass matrix factorization 
method of [9,11]. The development is done in the framework of classical mechanics, avoiding the need to 
work with the nonstandard spatial algebra of [4], and uses concepts of operator factorization and inversion 
which are well-known in the context of filtering and estimation theory [13,14]. 

As discussed in [9], the operator formulation and approach to robot dynamics is a powerful one with 
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a wide range of applications. This arises from an ability to give an operator interpretation to many 
analytical expressions which can then be directly mapped to equivalent recursive algorithms. The operator 
interpretation also enhances the ability to manipulate abstract analytical expressions by the use of operator 
identities which are derived in [9]. Although these other applications are not directly addressed here, note, 
for example, the straightforward way that a recursive construction of JM~ 1 J T , with J a manipulator 
Jacobian and M a manipulator mass matrix, is obtained in Sec. 6. [J.M -1 J 3 ’]" -1 is precisely the operational 
space manipulator mass matrix of [15], and in a similar way, recursions for computing other terms associated 
with the operational space formulation of [15] can be derived. 

2. PROBLEM FORMULATION AND STATEMENT 

The spatial notation and quantities of [9] are used throughout this paper, and [9] is therefore a 
fundamental reference for this paper. Results from [9] involving operator factorizations, inversions, and 
identities are used without proof. As in [9], the algorithms of this paper are given in coordinate free form. 
In actual applications, the algorithms can be projected onto fixed-link frames as is done in [3,7]. Table 2.1 
provides definitions for many of the variables used in here. 

The system of interest consists of l rigid-link, possibly redundant, manipulators grasping a commonly 
held rigid object in Euclidean 3-space. This is the same setup considered in [2] and Fig. 2.1 represents this 
situation. The grasp contact points are at O = col(Oi ,. . .,Og), and (in the body of this paper) the grasps 
are taken to be completely rigid, in the sense that no contact degrees of freedom are left unconstrained [16]. 
The rigid-grasp assumption is made to reduce notational clutter and is relaxed in Appendix B, where a 
variety of possible grasp contacts is considered. The set of admissible contacts used in Appendix B is given 
in Appendix A. The effect of gravity loading is ignored with no loss of generality as gravity can be dealt 
with in the standard way [7,9] by giving an appropriate pseudo- spatial acceleration to the manipulator base 
(although care must be exercised to separate true spatial accelerations from pseudo-spatial accelerations). 
With the notation of [9], and the quantities defined in Table 2.1, the dynamics of a single IV-link arm i, 
with » = 1 , ... ,1, is given by 


HA + Ci + JTfm = Ti 

(2.1a) 

Xj(0) = Qi(0i) is 6-dimensional 

(2.16) 

v;(o) = M e R 6 

(2.1c) 

<*i(0) = Vi(o) = JA + jJi e R 6 

(2-1*0 
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0*, 0*, $i are iV-dimensional. 


Note that every arm is taken to have the same N degrees of freedom when unconstrained by contact. This 
assumption is made only to simplify the notation and results in no real loss of generality. N itself can take 
on any value, subject to N > 6. -X,*(0) gives the spatial position (orientation and location) of the tip of 
arm t and evolves on a 6-dimensional manifold which is usually R 6 or R 3 x 50(3). V^(0) is the spatial 
velocity, and 0^(0) = Vi(0) is the spatial acceleration of the tip of arm t\ The contact force imparted to 
the held object is /;( 0). 

To succinctly describe the dynamics of all £ arms, define for * = 1 , . . . , £ 

.M = dtag(Mi), J = dtag(Ji), C = co/(C t ), 

0 = co/(0 t ), T = col(Ti) y f(0) — co/(/ t (0)), Q = col(Q i ) 

X(0) = col[Xi{0)) 9 V(0 ) = col[V i (0)l a(0) = col[ai{ 0)], 

and 

n = tN. (2.2) 

n gives the total number of manipulator links in the system of Fig. 2.1. Note that 0 is n-dimensional, 
M £ R nXn y 1^(0) € R Qt y J € R QtXn y etc. With the above definitions, the aggregate arm dynamics are 
given by 

Ml + C + J T f = T (2.3a) 

X(0) = Q{0) (2.3 b) 

F(0) = J9 (2.3 c) 

a( 0) = JO + JO. (2.3d) 


With C a point fixed with respect to the held object, let M(C) be the held object’s spatial mass 
referenced to the point C. M(C) is given by 


i (C)=( Ic -. 


-rn c pc rn G I J 

where me is the held object mass, I G is the object rotational inertia tensor at C, and pc is the 3- vector 
from C to the object mass center. Here, vy ~ v x y. In the spatial notation of [9], the object dynamics are 
given by 

M(C)a(e) + b(C) = f(C), (2.4) 
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where 


w={ m z:\z c 7 P <,)) “ d /(c > = (£) 

are the bias spatial force and the resultant net spatial force at the point C respectively. Equation (2.4) 
is a succinct way of summarizing the coupled Newton and Euler equations which describe the rigid body 
dynamics of the held object [17]. 

With £ arms grasping the object at the £ distinct contact points Oj for j = 1, ...,£, there sure t — 1 
independent loops formed (the base is viewed as being common to Jill arms), and t is also a measure of 
the number of closed kinematic chains for the sytem represented by Fig. 2.1. This can be seen by noting 
that cutting any t — 1 distinct arms will result in £ distinct and independent (i.e., kinematically decoupled) 
serial link systems, and that at least £ — 1 cuts are needed to remove all loops. Assuming rigid arms, a 
rigid central body, and rigid grasps, the kinematic constraints imposed by the £ arms grasping the object 
at the contact points are holonomic [18] and consist of the requirement that the total change in position 
and orientation as one traverses around any loop of the system be zero. Differentiating these constraints 
leads to the following natural constraints on spatial velocities and accelerations for t = 1, . . . ,£. 


Vi( 0) = <f> T {C,Oi)V{C) 


(2.5a) 


a,( 0) = <t> T (C,Oi)a(C) + a<(C) 


(2.56) 


where 


« c '°')=(o *'< C >-Uxh» 0 *«c.a l )i) 


Here, £(C, Oj) is the 3-vector from point C to point O,-. Since <f> 1 (a, 6) = <f>(b, a) and <f>(a, b)(f>(b, c) = 4>{a, c) 
[6,9,10], (2.5a) can be rewritten as V(C) = <f> T (Oi,C)Vi(0) so that for every i,j = !,...,£ 


<f> T (O jt C)V 3 i 0) - <f> T (Oi,C)Vi(0) = 0. 


Equivalently, 

Vj(o) - 4> T (Oi,Oj)Vi(o) = o. 

This is just the requirement that the total change in velocity as a loop is traversed is zero, which is consistent 
with the original holonomic constraints. Equation (2.5b) follows from (2.5a) by direct differentiation, i.e. 

V<(0) = a<(0). 
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Defining 

A t = [<f>(C, Oi), • • • , <f>(C, Ot)\ G R« xei (2.6) 

and a(C) = co/[ay(C)], Eqs. (2.5) become 

V^(0) = AV(C ) (2.7a) 

a(0) = Aa(C) + a(C). (2.7 6) 

Because <f>(C,Oi) is invertible, it can be shown that A T A = ^f =1 <f>{C ,Oi)<f r {C ,0{) € i? 6x6 is invertible. 
This enables the determination of V(C) G R 6 from V (0) G R et and the constraint relation (2.7a) by 

V(C) = {A t A)~ x A t V{ 0) (2.8a) 

and a(C ) from a(0) and (2.7b) by 

a(C) = (A r A)- 1 A r [a(0) - a(C)]. (2.8t) 


The resultant net spatial force at C due to the contact forces fi(0) is given by 

/(<?) = 

t=l 


or 

m = A r m. 


The complete closed-chain dynamics are given by (2.3), (2.4), (2.7b), and (2.9) or 


(2.9) 


MO + C + J T f{0) = T (2.10a) 

a(0) = JO + J0 = Aa(C) + a(C) (2.106) 

M(C)a{C) + 6(C) = A T f( 0). (2.10c) 


When integrating the system (2.10), the holonomic loop constraints and the velocity constraints (2.7a) 
must be satisfied. Equations (2.10) can be written as 


/ 0 J -A \ 
J T M 0 
\-A t 0 M(C) J 


m 


a(C) -J0\ 
T-C . 

-m J 


( 2 . 11 ) 
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From (2.10c), a(C ) = M~ 1 (C)A T f( 0) - M~ 1 (C)b(C), which with (2.10b) yields 


a(0) = AM~ 1 (C)A T f( 0) - AM- X (C)6(C) + a(C). 


With (2.12) substituting for (2.10b) and (2.10c), Eqs. (2.10) can be simplified to 


or 


where 


MO + J T f( 0) = T-C 

JO - AM~ 1 (C)A T f( 0) = a(C) - AM _1 (C)&(C) - JO 

(j -Am\c)A t ) ( /(% ) = (J[c)) 

T' — T - C 

a'{C) = a(C) - AM -1 (C)6(C) - JO. 


( 2 . 12 ) 

(2.13a) 

(2.136) 

(2.14) 

(2.15a) 

(2.156) 


We now state the 

CLOSED-CHAIN FORWARD DYNAMICS PROBLEM (CCFDP): 

Given joint data (0,0) and input moments T, find the accelerations 0 and the contact forces /( 0). 

Note that from (0,0), V(0) = JO and V(C ) = (A r A) _1 AV(0) can be computed enabling the determi- 
nation of a(C) and 6(C) as well as the determination of the (9, 0)-dependent terms M,J,J0,C,M(C), and 
A. Thus knowledge of (0,0) and T is sufficient for knowing the right-hand-sides of Eqs. (2.11) and (2.14) 
and the coefficient matrices of (2.11) and (2.14). The CCFDP is solved, then, by any (0,f( 0)) for which 
(2.14) holds with M,J,AM~ 1 (C)A t ,T', and a'(C) determined by T and (0,^). Similarly, (0,f( 0)) and 
a(C) can be determined as the solution to (2.11). Note that having found (0,f( 0)), a(0), we can compute 
a(C') and f(C) from (2.3d), (2.8b), and (2.9) respectively. 

Equation (2.11) is essentially [2; eq. 39] couched in the spatial notation of this paper and specialized 
to the case of several arms grasping a common object. (Reference [2] also deals with the more general case 
of multilegged locomotion). Note that the coefficient matrix of (2.11) is symmetric - a fact not readily 
evident in [2] due to the more general notation used. This matrix is certainly not positive definite due to 
the leading 0 on the block diagonal. The solution to (2.11) is seen to be equivalent to the simpler system 
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(2.14). In both cases, the nonsingularity of the coefficient matrices is not obvious. In [2], it is proposed 
that (2.11) be solved for /(0), 0, and a(C ) by first determining the coefficient matrix and the “bias” terms 
by repeated application of the recursive Newton- Euler algorithm (in the manner first proposed in [3]), after 
which (2.11) is solved by Gauss-Jordan elimination or by the use of the Gauss-Seidel method. To solve for 
(/(0), 0, a(C)) in this manner requires 0(N 3 f?) = 0(n 3 ) operations [2]. 

The same approach can be used to solve the simpler system (2.14), resulting in the same complexity 
of 0(n 3 ). Rather than do this, however, a solution for (2.14) is found for which the invertibility issue of 
the coefficient matrix is clearer. 


3. AN ANALYTICAL SOLUTION TO THE CLOSED-CHAIN 
FORWARD DYNAMICS PROBLEM 

Define 0 6 R etxGt by 

n = JM~ 1 J T + AM- 1 (C)A T . (3.1) 

Premultiplication of both sides of (2.14) by 

( 1 

\-JM~ 1 l) 

results in 

(o -n) ( /(0) ) = {-/m-* ° ) (a'^C))- < 3 - 2 > 

If n is nonsingular, Eq. (3.2) is certainly solvable. More generally, (3.2) is solvable if the right-hand side 
of (3.2) is in the image of the coefficient matrix. This more general case is not considered here, and 0 is 
assumed to be invertible. Further discussion on the relationship of the singularity of f2 to the solvability 
of the CCFDP is deferred to the next section. 

Premultiplication of (3.2) by 

(! T‘) 

yields 

" ( T> \ 

-JM- 1 I I ( a'(C ) I I 3 ' 3 ' 


(M o \ ( 9 \ _ ( I - JTQ-'JM- 1 J^n- 1 

VO -n) {m)-{ - 


or 


UM 


.M - 1 - >r 1 J r n - 1 jm- 1 

q-'jm - 1 


>r 1 J T n - 1 

-n- 1 


r 

a'(C) 


(3.4) 
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In other words, 


(M J T \ _1 _ ( M- 1 - M-Wn-iJM- 1 M -1 J T n -1 \ 

\j -AM" 1 (c)A r y v n-VAi -1 -rr 1 ) 

_{M~ 1 0 \ fM-J T Cl~ 1 J J T \ ( AT 1 0 \ 

- { o n- 1 / v J -ny v ° n~v * 

Eq. (3.4) is the analytical solution to the CCFDP given by (2.4), which in component form is 

9 = (AT 1 - + AT 1 J T 0" 1 a , (C) (3.6a) 

/(0) = - n _1 a'(C) (3.66) 

where T' and a' are given by (2.15). In this paper, Eq. (3.6b) is viewed as equivalent to the more general 
0/(0) = — a'(C) found from (3.2). 

The solution (3.6) is just the solution that arises from (2.13) by substituting 6 determined by (2.13a) 
into (2.13b) and then solving for the constraint forces /( 0). This gives (3.6b). After this step, the solution 
for /( 0) is substituted into (2.13a) resulting in an equation involving only 0 which can be solved to give 
(3.6a). This approach of first solving for the constraint forces and then obtaining an equation purely in 
terms of 9 is precisely that suggested in [19]. Reference [19] states this procedure abstractly, after which it 
is applied to the constrained equations arising from bipedal motion. This technique is also used in [20] to 
obtain the equations describing various robotics applications, including the case of two robot arms carrying 
a common object for which equations similar to (3.6) are derived. For now we will defer the analysis of 

(3.6) as a means of obtaining recursive closed-chain forward dynamics algorithms until Section 6. The 
following Section 4 gives a general algorithm for solving the CCFDP based on an alternative derivation to 

(3.6) , after which the relationship of this general algorithm to (3.6) will be discussed. 

4. A GENERAL ALGORITHM FOR SOLVING THE CLOSED-CHAIN 
FORWARD DYNAMICS PROBLEM 

For given (9, 9) and T, the “free dynamics” Me defined by 

M9 f + C = T (4.1a) 

a/(0) = J9 f + J9. (4.16) 


(3.5a) 

(3.56) 
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Eqs. (4.1) should be compared to Eqs. (2.10). 9f and ctf(0) are the joint accelerations and arm tip 
spatial velocities that would occur if the arms were not constrained by contact with the object. Also, define 

AO = 6- 6j (4.2a) 

Aa(0) = a(0) - a,(0) (4.26) 

so that 0 = 6j + AO and a(0) = Of/(0) + Aa(0). 

Equations (2.13a), (4.1a), and (4.2a) yield 

MAO = - J T /( 0) (4.3) 

or 

AO = - JT 1 J T /(0). (4.4) 

Equations (2.12), (2.13b), (4.1b) and (4.2b) yield 

JAO = Aa(0) (4.5) 

with 

Aa(0) = AM~ 1 (C)A T f(0) - AM -1 (C)b(C) + a(C) - a f (0). (4.6) 

Again the matrix fi € R 6tx6t is defined by 

n = JM-'J 7, + AM~\C)A t . (4.7) 

Eqs. (4.4)-(4.7) combine to give 

0/(0) = o/(0) + AM _1 (C)6(C) - a(C) (4.8) 

Equations (4.1) and (4.8) completely determine the tip contact forces acting on the held object. 

Equations (4.1)— (4.8) together result in the following general 4-step closed-chain forward dynamics 
(CCFD) algorithm. 

GENERAL CCFD ALGORITHM. 

1 ) Find the free accelerations (0/,a/(0)). 

M0/ + C = T (4.9 a) 
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ctf(O) = J9 f + J0 


(4.96) 


2) Find the constraint contact forces, /( 0). 


n = JM-'J 7 + AM~ 1 (C)A t (4.9c) 

n/(0) = a f (0) + AM~ 1 (C)b(C) - a(C). (4.9<f) 

3) Find the correction accelerations, (AP,Aa(0)). 

MAP + J T f( 0) = 0 (4.9e) 

Aa(0) = JAP. (4.9/) 

4) Find the true accelerations, (P,a(0)). 

P = Of + AP (4.9$) 

Aa(0) = ctf (0) + Aa(0) . (4.9 h) 


It is evident that the key step is the ability to solve (4.9d) for the constraint forces /( 0). The solvability 
of (2.11) or (2.14) is evidently equivalent to the solvability of (4.9d), which is equivalent to the requirement 
that the right-hand side of (4.9d) be in the image of fi. This most general condition for solvability will 
not be investigated, except to say that it is conjectured that for any trajectory of the closed-chain system 
which satisfies the kinematic constraints, the right-hand side of (4.9d) will start and remain in the image 
of Q. From (4.9c), it is seen that a sufficient condition for the invertibility of fl is that all arms be 
kinematically nonsingular, so that J is of full rank and JM -1 J T is therefore invertible. The matrix fl -1 
is a generalization of the Operational Space mass matrix of [15] and represents the mass of the system 
reflected to the contact points. 

Note that any available forward dynamics algorithm can be used to solve (4.9a), (4.9b), (4.9e), and 
(4.9f). For example, these equations can be solved using analytical expressions for arm dynamics when 
available; or, they can be solved by repeated applications of the recursive Newton-Euler algorithm, as in 
[1- 3] (to determine J, M, C, JO, Of, and c*/(0), after which fl is inverted to obtain /( 0), AP, and Aa(0)); 
or finally these equations can be solved using recursive algorithms which directly yield Of, AP, a/(0) and 
Aa(0) such as those in [4] or in [9]. 
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To show how the general CCFD algorithm (4.9) can be obtained from (3.6) with T' and a' given by 
(2.15), write (3.6b) as 

nf(0) = JM~ 1 T , -a'(C) 

= J0 f - a{C) + AM~ x {C)b(C) + JO. 

= a/(0) -I- AM~ x {C)b{C) - o(C). 

Additionally, (3.6a) can be written as 

9 = M~ X T' - M-'ji'Cl-'iJM-'T' - o'(C)] 

= Of - M~ x J T f{ 0) by (3.6b) 
or 

MAO + J T f{ 0) = 0. 

The point to be drawn here is that the algorithm (4.9) is only one of several possible algorithms 
obtainable from (3.6), depending on how terms are grouped and interpreted. This is particularly pertinent 
since, due to the work of [6,9-12], it is known that the coefficient terms in (3.6) can be viewed as operators 
which can be combined in a variety of ways using the operator factorizations, inversions, and identities 
to be found for example in [9]. Although it is true that algorithm (4.9) has a particularly nice form and 
interpretation, as we shall see it is by no means the only way to implement the solution given by (3.6). 

5. AN OVERVIEW OF OPERATOR METHODS FOR SOLVING 
THE FREE FORWARD DYNAMICS PROBLEM 

Consider the dynamics of a single arm given by (2.1). In [9], it is shown that (2.1) has an interpretation 
as an operator- formulated dynamics. From this perspective M, and J, are viewed as operators of the form 

Mi = HitiMitfHf 

and 

Ji = BTtfHf 

where the specific definitions of the component operators Hi, B,, fa, and M, can be found in [9]. Ci and 
JiO, also have the forms 

C* = HiMMitfai + bi) 
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and 

jJi = B? <f>l CLi + CLi( 0) 

where [aj,a,(0)] are the “bias spatial accelerations” and 6, are the “bias spatial forces” associated with the 
joints of arm t. Ci and Jjflj are seen to arise from the action of certain operators acting on the biases a, 
and bi. 

Mi and H, are block diagonal and are therefore said to be “memoryless” . <£, is a lower block-triangular 
matrix and is therefore said to be “causal” . A causal operator is equivalent to a recursive algorithm which 
processes link data in an iterative sweep which goes from the arm tip to the arm base. <j>J is upper block 
triangular and therefore “anticausal” . The action of the anticausal <f>J on link quantities is equivalent to a 
base-to-tip iteration. As in [9], a link numbering scheme is used which increases from the tip (on link 1) to 
the base (link JV + 1). For this reason a base-to-tip anticausal recursion iterates from k=N + ltok = l. 

A statement like Vi(O) = Bj tf>J Hj says that the tip velocity V,(0) is equivalent to an anticausal 
(base-to-tip) processing of the joint rates 6 <. The action of Bj is to pick out the spatial velocity associated 
with link 1 at joint 1 and propagate its effect to the tip. The equivalent recursive formulation is 

Vi(N + 1 ) = 0 ; 


for k = N, . . . , 1 loop 

Vi(k) = <j>J(k + l,k)Vi{k + 1) + H? (k)9i(k) 

end loop 

Vi(0) = ^f(l,0)Vi(l) 

where HTft) gives joint axis k of arm i and + l,k) is the interlink Jacobian for link k + 1 of arm i. 

Since the product Hi<f>{ is lower block triangular, or causal, = Hi4>iMi<f>J Hf is seen to be an 
operator with a causal-memoryless-anticausal factorization. 

Now let H — diag(Hi), <f> = diag(<f>i), M = diag(Mi), B = diag(Bi ), a = co/(a,), a(0) = co/[aj(0)], 
and b = col(bi). The aggregate dynamics (2.10) will be given an operator interpretation by taking 

Af = H<l>M<f> T H T (5.1a) 

J = B t <I> t H t (5.16) 
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C = H<j>(M<f> T a + l >) 
jO = B T (f> T a + o(0). 


(5.1c) 
(5. Id) 


Note that the aggregate forms (5.1) have precisely the same structure as the single arm case. This is 
always the case for the operator forms extended from the single arm case studied in [9] to the multiarm 
case considered in this paper. Thus all operator factorizations, inversions, and identities developed in [9] 
hold here, with the understanding that an operator expression used here stands for the parallel application 
of this operator to all arms. As an example, 

F(0) = JO = B T H T <t> T 0 

is equivalent to 

V<(0 ) = J i b i = BjHl<fibi for « = 1,...,£. 

A more detailed and formal, but somewhat different, extension from single arms to multiple arms may be 
found in [6]. 

In [9], it is shown that X has an alternative causal-memoryless-anticausal factorization given by 

X = ( I + H$L)D(I + H$L) t (5.2 a) 

where <$,, Li, and £), correspond to arm t, as defined in [9], and $ = diag($i), L = diag(Li), and 
D = dtag(Di). Eq. (5.2) is just the aggregate statement that for t = 1, . . . ,1 

Mi = (J + Hi*iLi)Di{I + HiQiLif (5.2 b) 

and it is actually (5.2b) that can be found in (9). $ is a strictly causal operator which is related to <f>. L 
and D are memory less and are specified by a causal tip-to-base recursion given in [9]. It is shown in [9] 
that D is invertible and that 

( I + HQL)- 1 = (J - HVL) (5.3) 

where 'i' is a strictly causal (strictly lower block triangular) operator which is determined from <f>, L, and 
H. Equation (5.3) is a statement about two causal operators, (J + H$L) and (7 — H'PL), being causal 
inverses of each other. 

Equations (5.2) and (5.3) result in the important operator factorization of the inverse mass operator, 

X" 1 = (J - H*L) T D- l {I - HVL). (5.4) 
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The action of A1 _ 1 is equivalent to a causal tip-to-base filtering recursion given by D~ 1 {I — H^L), followed 
by an anticausal base-to-tip smoothing recursion given by ( I — H^L) 7 . For each arm all of these actions 
are O(N), and thus the action of X -1 is 0(n) = O(tN). 

With (5.1), the closed-chain dynamics (2.13)-(2.15) eu-e given by 


T' = T - H<f>{M<j> T a + b) 

(5.5a) 

a'{C) = a{C ) - AM-'iCWC) - B T <j> T a - a(0) 

(5.56) 

H<t>M<i> T H T e + 0) = T' 

(5.5c) 

B T fH T 9 - AM~ 1 (C)A T f(0) = a'(C). 

(5.5d) 

For brevity, usually the expressions (2.13)-(2.15) are written, with the understanding that they have 
the interpretation of (5.5). This is the sense in which (2.13) is said to provide an “operator-formulated” 
closed-chain dynamics. 

The “free dynamics” (4.1) can be written as 


H<f>M<j> T H T 9 f = T - H<t>{M<j> T a + b) 

(5.6a) 

ctf( 0) = B^ <f>T H t 9 f + B t <$> t a + a(0). 

(5.66) 

Equation (5.6a) can be equivalently written as 


T' — T — H<t>{M<f> T a + b) 

(5.7a) 

H<f>M<t> T H T 9 f = T'. 

(5.76) 


Whether one chooses to solve (5.6a) or (5.7b) results in a different algorithmic implementation. To solve 
(5.7b), the “bias” moments C = H<f>(M<f> T a+b) are first removed by a 2-sweep (a base-to-tip sweep followed 
by a tip-to-base sweep) Newton-Euler recursion [9]. This approach is now standard and is used in [1-3] 
and [4,5]. With (5.6a), different algorithmic options are available as discussed in [9]. 

With (5.4), (5.7) has the solution 


T' — T - H<j>(M<t> T a + b) 


(5.8 a) 
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Of = (J - HVLfD-'V - H*L)T'. 

(5.86) 

Equation (5.8b) can be written as 

e = (J - H*L)T' 

(5.9a) 

v = D~ 1 e 

(5.96) 

0/ = (I - HVL) t v. 

(5.9c) 

For the i th arm (5.9) is equivalent to the following recursions [9] 


?.(0) = 0, 77(0) =0 

(5.10a) 

for k = 1, . . . , N loop; 


Zi(k) = xl>i(k, k - l)zi(k -1) + Li{k- l)Ti(fc - 1) 

(5.106) 

€i(k) = Ti(k ) - Hi(k)zi{k) 

(5.10c) 

i 'i(k) = D^ 1 (k)c i (k) 

(5.10d) 

end loop; 

A,(AT + 1) = 0 

(5.10e) 

for k = 1, . . . , N loop 

A i(k) = rjjf ( k + 1, k)\i(k + 1) + Hj ( k)vi(k ) 

(5.10/) 

0 if (k) = Vi(k) - Lf ( k)Xi(k + 1) 

(5.10 ff ) 


end loop; 

Here, the matrix tpi(k + 1, k) is defined as 

tpi{k+ 1 y k) = <f>i (k + l 9 k) - Li(k)Hi(k). 

Eq. (5.10b) is a Kalman filter and Li(k) are the Kalman filter gains. Eq. (5.10c) computes the white- 
noise residuals e*(fc). Eq. (5.10b) and (5.10c) show a Kalman filter being used as a causal whitening filter. 
Eq. (5.10d) computes the weighted residual Ui(k), Eqs. (5.10e) — (5.10g) show an anticausal filtering, or 
smoothing, action which processes the weighted residuals to obtain 0 t/ . 
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Such filtering and smoothing ideas underlie the results of this section, and are an extension of filtering 
and smoothing concepts from linear estimation theory [13,14] to mechanical multibody systems [6,9-12]. 
The key concept which is exploited to make this extension in [6,9-12] is the linear relationship which exists 
between accelerations and forces for mechanical systems. 

For notational simplicity, what has not been made explicit in (5.8) is the need to perform the recursions 
given by V = B T <f> T H T 9 in order to compute the velocity-dependent biases a and 6, as well as the need 
for recursions to compute D, L, and As discussed in [9], the necessary operations can be performed 
simultaneously with the explicitly stated recursions. 

If (5.6a) is directly solved by an application of (5.4), operator identities found in [9] and various 
groupings of terms can be used to place the solution in a number of alternative forms, each having a 
different algorithm implementation. For example, with (5.4), (5.6a) becomes 

9 f = (J - tf*L) r ir 1 (J - H9L){T - H<p(M<p T a + b)}. (5.11) 

If the terms in the curly brackets are kept distinct and computed before the action of (I — HVL), then 
the solution of (5.8) -(5. 10) is applicable. On the other hand the identities 


* = ips 

(5.12a) 

( J - H*L)H<p = Hip, 

(5.126) 

where ip and S are operators defined in [9], enables (5.11) to be written as 


9 / = (I - HVty-'D-'iT - Hip(SLT + M<P T a + 6)}. 

(5.13) 

Eq. (5.13) is equivalent to 


£ = <p T a 

(5.14a) 

e = T- Hip(SLT + Aff + b) 

(5.146) 

v = D~ x t 

(5.14c) 

9/ = (J - H*L) t v. 

(5.14d) 


The algorithm represented by (5.14) is significantly different from that of (5.8a) and (5.9). In particular, 
the algorithm (5.8a) and (5.9) requires four base-to-tip or tip-to-base sweeps (two to compute T' in (5.8a) 
and two to compute 9j via (5.9)), while the algorithm of (5.14) requires only three sweeps. 
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The identities of [9] can also be used to obtain aj(0) of (5.6b) simultaneously with the computation 
of Of from (5.9) or (5.14), resulting in additional computational savings. However, this possibility is not 
exploited in this paper. The details may be found in [9]. 

For additional simplicity, in the remainder of this paper, the focus will be on the algorithmic possibil- 
ities obtained from removing the bias moments in the manner (5.5a), and working with bias-free moments 
T', and the bias-free dynamics given by (5.5c) or (5.7b). Even with this (unnecessary) restriction, it will 
be seen that there remain a variety of algorithmic possibilities to solving the CCFDP. 

6. RECURSIVE CLOSED-CHAIN FORWARD DYNAMICS 
BY OPERATOR METHODS 

With the background of Sec. 5, first consider the general CCFD algorithm (4.9). Step 1 of (4.9) can 
be recursively performed by (5.8a), (5.8b), and (5.6b), resulting in 

T' = T - H<f>{M<t> T a + b) (6.1a) 

0 f = (I - ff^L) r D~ 1 (I - H*L)T' (6.16) 

ay(0) = B T <f> T H T 0f + B T 4> T a + a(0). (6.1c) 

Eqs. (6.1) represent l recursive algorithms, each with a cost which is O(AT). See Eqs. (5.8) — (5.10). This 
results in a total cost of 0(lN) = O(n) computations. As was noted in the last section, (6.1) can be 
simplified by the application of the techniques in [9]. In particular, (6.1b) and (6.1c) can be combined to 
generate a single recursive algorithm for obtaining Of and a/(0). 

Step 3 of (4.9) involves solving (4.9e). Using (5.1b) and (5.4), (4.9e) becomes 

AO = -(I- H*L) T D~ l {I - H<bL)H<f>Bf{0). (6.2) 

With the identity (5.12b), (6.2) becomes 

AO = -(J - H^/L) T D- 1 Hrl>Bf{Qi). (6.3) 

Using the techniques of [9], (6.3) can be shown to be equivalent to a causal tip-to-base recursion followed 
by an anticausal base-to-tip recursion. For each arm t, (6.3) is equivalent to the O(N) recursion 

U 1) = M1,0)M0) (6.4 a) 
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For k = 2, . . . , N loop 


Zi(k) - <pi(k, k- 1 )zi(k - 1); 

(6.46) 

Vi{k) = D^ 1 (k)H i (k)zi(k)-, 

(6.4c) 

end loop; 

A i(N + 1) = 0; 

(6.4d) 

For k = N, . . . , 1 loop; 

A i(k) = ipf {k + 1, k)\i(k + 1) + Hj ( k)vi{k ) 

(6.4e) 

A $i = Vi{k ) - Lj ( k)Xi(k + 1) 

(6.4/) 


end loop; 

Equations (5.10e)-(5.10g) and (6.4d)-(6.4f) are equivalent as they both represent the action of the 
operator (J — H'bL) T . Compare (5.8b) and (6.3). 

Step 3 of (4.9) is solved then by the 0(lN) = O(n) recursions implied by 


A 9 = -(I - H*L) T D- x ipBf{ 0) 

(6.5a) 

Aa = B T tp T H T A0. 

(6.56) 


Again, (6.5a) and (6.5b) could be combined into a single algorithm using the techniques of [9]. 

So far Steps 1 and 3 of (4.9) involve 0(n) operations. The next task is to show that J M~ 1 J T , which 
is needed to obtain Q of (4.9c) can be obtained by an O(n) recursion. Note that J M ~ 1 J T has J, .M ~ 1 Jj 
on the block diagonal where (JiM^ 1 J?)' 1 is precisely the operational space effective mass of [15] for arm 
t. Use of (5.1b) and (5.4) leads to 

JH~ X J T = B t 4> t H t (I - H*L) T D~ X {I - H*L)H<t>B. ( 6 . 6 ) 

With the identity (5.12b), (6.6) becomes 

JM~ X J T = B T ip T H T D~ 1 HipB. (6.7) 

With the definitions of B, ip, H, and D to be found in [9], (6.7) can be shown to be equivalent to 

JM-'J 7, = A(0) (6.8a) 
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A(0) = diag[Ai(0)] 

where A^O) = for each arm » = 1 is given by 


( 6 . 86 ) 



A i (0) = ^(l,0)A < (l)^(l,0) 

(6.9a) 


A»(l) - jr rl>7( k , l) H ?(k)D7 x (k)HT (fc)^(fc, 1) 

L.— 1 

(6.96) 

which is equivalent to the O(N) recursion 



A i{N + 1) = 0 

(6.10a) 

for k = N 1 loop 



A i(k) 

= J,T(k + l,k)A4(k+l)4>i{k + l,k) + Hj (k)D^ x Hi(k) 

(6.106) 

end loop; 

Ai(0) = 4>f (l,0)Aj(l)^,(l,0). 

(6.10c) 

With £ arms, the computation of JM X J T via (6.8) and (6.10) is O(tN) = 0(n) 


With JAi -1 J T available, then fi = JM l J T + AM 1 (C)A r , and /( 0) can be solved from (4.9d). 

Since f2 is 6£ x 6£, this is an 

0(£ 3 ) operation. 


Together the above yield the following 0(n) + 0(£ 3 ) = 0{tN) + 0(£ 3 ) recursive 

form of (4.9): 

ALGORITHM RCCFD 

i. 


1) 

T' — T - H<t>{M<j> T a + 6) 

(6.11a) 


Of = (I - H*L) t D- x (I - HVL)T' 

(6.116) 


a,(0) = B T (j> T H T 9j + B T <f> T a + o(0) 

(6.11c) 

2) 

JM~ X J T = B T ip T H T D~ 1 Hxf>B 

(6. lid) 


n = JM' 1 ^ + AM~ 1 {C)A t 

(6. lie) 


/( 0) : G/( 0) = a,(0) + AM -1 (C)6(C) - a(C) 

(6.11/) 

3) 

A 9 = (J - H^L) T D~ 1 rpBf{ 0) 

(6.11®) 


Aa = B T <t> T H T A9 

(6.11 h) 

4) 

9 = 9 f + A9 

( 6-110 


Aa(0) = a/(0) + Aa(0) 

(6-11/) 
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Now turn to the solution (3.6) viewed as operator equations. As discussed in Sec. 4, the algorithm 
(4.9), and hence its recursive form (6.11), is just one way that the general solution (3.6) can be interpreted. 
Alternative groupings of terms in (3.6), and the use of different operator identities from [9], will result 
in many different O(n) + 0(£ 3 ) RCCFD algorithms, where the 0(£ 3 ) complexity arises from the need to 
solve for /(0), after fi has been obtained by an 0(n) recursion. It may well be that solving for /( 0) can 
be found to require less than 0(£ 3 ) operations by exploiting the special structure of fi. But for now, the 
inversion of fi, or the solution of the linear system of equations (6.1 If), will be assumed to be done using 
traditional 0(£ 3 ) methods. 

An alternative algorithmic possibility for obtaining the solution (3.6) is now demonstrated. From 
(5.1b), (5.4), and (5.12b) 

JM- 1 = B T tl> T H T D~ 1 (I - HVL). (6.12) 

With (6.12), (3.6b) becomes 

/( 0) = n~ 1 B T rP T H T D- 1 {I - H<ftL)T' - fT V(C) 

= fi - 1 {B T il> T H T D- 1 {I - H*L)T' - o'(C)}. (6.13) 

Eq. (6.13) is a useful form of (3.6b) which shows how the tip contact forces imparted to the held object 
can be obtained by: (1) A causal O(n) filtering of the bias-free torques T‘ to produce v — D~ 1 (I - HVL), 
followed by; (2) An anticausal O(n) filtering of u, given by the operation B T rp T H T v, where during this 
operation the O(n) recursions necessary to produce fi can be done, and (3) an 0(£ 3 ) inversion of fi to 
produce /(0). With (3.6b), (3.6a) can be written as 

0 = X _1 (r' - J T /(0)] (6.14) 

which, of course, can be found directly from (2.13a) and (2.15a). With (5.12a) and Af -1 given by (5.4) 
note that 

Ar 1 T' = (J - HVtfD-'V - HrpSL)T' 

= (J - HVLfD-'iT' - H^SLT'}. (6.15) 

Also, from (6.12) note that 

AT 1 J t /( 0) = (J - HVLfD-'HjBfi 0). (6.16) 
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Eqs. (6.14)-(6.16) result in 


0=(I - H<ZL) t D- 1 {T' - Hrl>{SLT' + B/(0)}. (6.17) 

Eq. (6.17) shows that a causal filtering of T' and /( 0), followed by an anticausal filtering results in 0. Eqs. 
(6.1a), (6.7), (6.13) and (6.17) yield the following algorithm. 

ALGORITHM RCCFD 2. 

T' = T- H</>{M<f> T a + 6) (6. 18a) 

= B T xp T H T D~ 1 Hxl>B (6.186) 

n = JA1 _1 J T + AM _1 (C)A r (6.18c) 

/( 0) = n - 1 {B t iP t H t D- 1 (I - H*L)T' - a'(C)} (6.18(f) 

6 = (I - H*L)D- l {T' - Htl>{SLT' + J3/(0)}. (6.18e) 

A third recursive algorithm can be obtained by taking 0 = Ox + 02 in (3.6a) where 

h = (AT 1 ~ Ar 1 J r n“ 1 JAr 1 )T' (6.19a) 

$2 = M~ 1 J T n~ 1 a'(C). (6.196) 

With (6.12), (6.19b) becomes 

0 2 = (J - H^L) r £>" 1 R'V’Bn" 1 a'(C), (6.20) 

while with (5.4) and (6.12), it is possible to write 

0i = (I - H*L) T {D~ l - D- 1 HrpBn~ 1 B T ^ T H T D- 1 }{I - H*L)T' (6.21) 

Eqs. (6.1a), (6.7), (6.20), and (6.21) result in 
ALGORITHM RCCFD3. 

T' = T - H<f>(M<f> T a + 6) (6.22a) 

J.M- 1 J T = B T ip T H T D~ 1 HrpB (6.226) 

H = JM~ x J t + AM~ 1 {C)A t (6.22c) 
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f (c) = rr V(c) (6.22 d) 

/( 0) = n^ 1 B T xl> T H T D^ 1 (I - H9L)T' - ?(C) (6.22e) 

0! = (J - H^L) T {D" 1 - I?“ 1 ^Bn-’ 1 B T ^ r ff T i?‘ 1 }(J - H*L)T' (6.22/) 

0 2 = (I - F^L) T i?" 1 ^5f(C). (6.22$) 

0 = 0*i “h 0*2* (6.22/i) 


Note that (6.22f) requires 4 causal or anticausal sweeps to produce 0\ from T'. Although algorithm 
RCCFD3 may be more computationally intensive than RCCFD1 or RCCFD2, it is an implementable 0(n)+ 
0(£ 3 ) recursive forward dynamics algorithm. The differences between the forward dynamics algorithms 
RCCFD1-RCCFD3 show dramatically the algorithmic variety and possibilities available from the tools 
provided by the operator perspective developed in [9] and here. Again, note that additional variations are 
possible which relax the need for the a priori determination of the bias-free moments T', and which allow 
the determination of the tip acceleration in the same iteration which produces the joint accelerations 9. 
The details may be found in [9]. 


7. CONCLUSIONS 

The exact analytical solution for the closed-chain forward dynamics problem of several arms grasping 
a commonly held object has been derived and is given by Eqs. (3.6). It is shown that the key step in 
obtaining the solution is the ability to solve for the tip forces /( 0), from Eqs. (3.2) or, equivalently from 
(4.9d). The solvability question is related to the nature of fi given by (3.1), and a sufficient condition 
for solvability is that JM~ X J T be full rank, which is true if no arm is at a kinematic singularity. A 
general algorithm, given by Eqs. (4.9), can be obtained from (3.6) which can be implemented using any 
available open-chain forward dynamics solver, analytical or recursive. In particular, in this paper this 
algorithm is implemented recursively using the linear operator formulation of robot dynamics and the 
operator factorization and inversion techniques of [9], More generally, the theory of [9] can be applied to 
the general analytical solution (3.6) to produce a variety of different recursive O(n) + 0(£ 3 ) closed-chain 
forward dynamics algorithms where £ is the total number of grasping arms and n = £N is the total number 
of manipulator links, each arm having N links, where N > 6. 

The primary motivation for using the operator approach is to allow an understanding of closed-chain 
dynamics at a higher level of abstraction than is possible with other methods. By use of the operator 
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methods patterns and mathematical structures begin to emerge that are otherwise not detectable. An 
example of this is the ability to express dynamics problems in terms of the spatial operator algebra of [9] 
and of this paper. The benefit is not only conceptual however, as the operator equations can be made 
specific to produce recursive algorithms that are readily implemented using the more detailed state space 
notation. 
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APPENDIXES 


APPENDIX A: CONTACT MODELS FOR NON-FULLY -CONS TRAINED GRASPS 


Only contacts Me considered for which the contact point does not slip on the object surface. As will 
be discussed, this still leaves several types of contact. The nonslippage assumption is related to the “force 
closure” assumption [16], which is the assumption that all contact forces are in an appropriate friction 
cone, so that friction forces Me active which prevent slipping. During a simulation this assumption must 
be continually verified. 

As in the body of this paper, here, and in Appendix B, a “stacked notation” is continued to be used in 
which a vector V (0) is a composite vector V (0) = co/[Vj(0)] for * = 1, . . . , t. A useful conceptual feature of 
this notation is that it allows viewing V (0) sis representing either all arm tip velocities or the tip velocity of 
any single arm (viewed generically) so that the distinction between V (0) and Vj(0) tends to disappeM. For 
this reason, the aim will be to avoid the subscripted notation. It is to be understood that the expressions 
written below and in Appendix B pertain to all arms as well as to each arm. 

Since the contact points Me assumed fixed relative to the held object, only 1, 2, and 3 degree of freedom 
rotational motions of the arm tips relative to the object at the contact point Me allowed. Equivalently, 
only 1 , 2, or 3 degree of freedom rotational motions of the object at the contact point relative to the arm 
tips Me allowed. At the contact point, then, the lineM and angulM velocities obey 

v"(0) = v + (0) 

w”(0) = w + (0) + Aw + , (a.l) 

where: u/ + (0) gives the arm tip angular velocities (“+” being the manipulator side of the contact); w“(0) 
gives the object angular velocities at the contact points ” being on the side of the contact away from the 
manipulators); and Aw + gives the relative angular velocity of the object (on the “— ” side of the contact) 
with respect to the arm tips (on the “+” side of the contacts). Equation (a.l) can be recast as 

k-(o) = k + (o) + ay + 

AV + = co/[Aw + ,0]. (a. 2) 

As the contact is crossed in an outwMd direction from the arm tip to the task object, imagine that the frame 
on the arm tip side (the “+” side) is left behind in order to enter a frame on the object side (“-”). The 
“+” and “— ” contact frames differ only by a rotation, R + = diag[R^ ] which is configuration dependent. 
The contact configuration pMameters are given by 0(0) = co/[0,(O)]. 
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At a given contact », 0^(0) £ R mi , where in general is different for each arm. However, for 
notational simplicity, take m t = m for all f, and ©(0) £ R fm . This results in no loss of generality, as 
the relevant equations written below can be taken as statements on an individual arm for implementation 
purposes. For nonrigid contacts m > 1, and, more generally, m > n g , where n g is the number of degrees of 
freedom of the contact which is assumed the same for each contact. In general, m can be larger than the 
number of grasp degrees of freedom, if we are to have a parameterization which gives a globally nonsingular 
representation of the relative attitude R + [21,22]. 

The “contact configuration velocity” associated with 0(0) is given by 

W(0) = co/[W<(0)] € R tn ° (a. 3) 

with PFj(O) £ R n <>. Here, 

©(0) = F[e(0)]W(0) (a. 4) 

where F[©(0)] £ and W (0) is related to Aw + by 

Aw+ = *(0)17(0) (o.5) 

with h[0, ©(0)] £ R 3tXn a e , Equations (a.2) and (a.5) result in 

F~(0) =F + (0) + H’ t ( 0)1V(0) (a. 6) 

with H t { 0) = co/[*(0),0] € j? 6<Xn s>*. h(0), and hence ff T (0), is assumed to be full rank. 

In order to make the above clearer, consider the following three types of contact. 

Case 1 : 1 dof rotational contact. A contact which forms a revolute lower kinematic pair has one rotational 
degree of freedom and is two-sided in the sense that the contact cannot be broken [23]. A kinematically 
equivalent contact is formed by an edge or line contact with friction, which is one-sided in the sense that 
the contact will be broken if appropriate contact forces normal to the object surface are not maintained 
[16]. Let h( 0) = diop[hi(0)] € R 3txi give the t unit 3- vectors about which the contact rotations can 
occur. Then, the configuration parameters are the angles 0(0) = 0(0) £ R l . This gives 17(0) = 0(0) and 
Aw + = h(O)0(O). Note that m = n g = 1. 

Case 2 : 3 dof rotational contact. A contact which forms a spherical (ball-in-socket) lower kinematic pair 
has a full three rotational degrees of freedom and is two-sided in the sense that the contact cannot be 
broken [23]. 
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A kinematically equivalent contact is formed by a point contact with friction, which is one sided 
since the contact can be broken if proper contact forces are not maintained [16]. There are many possible 
configuration parameters for this type of contact, corresponding to different representations of the rota- 
tion group 50(3). Minimal 3-parameter representations, such as Euler angles, have the drawback that 
they become singular for certain contact orientations. Moreover, this singularity is purely a mathematical 
problem having nothing to do with the possible contact motions which are completely rotationally uncon- 
strained. A commonly used 4-parameter representation which is globally nonsingular on 50(3) is given 
by the Euler 4-parameters (the unit quaternions) [21,22]. With the Euler parameters, global control of a 
3-dof rotational contact is possible with no control breakdown due to representation singularity [24,25], 
In either case, n g = 3 and we take W(0) = Aw + , h( 0) = I G R 3tx3t . If the Euler parameters are used, 
m = 4 and 0(0) = g(0) G R il , while if the Euler angles are used instead, m — 3 and 0(0) = (3 G R 3t . The 
appropriate relationship (a.4) may be found in [21,22] or in [24]. 

Case 3 . 2 dof rotational contact. The “soft finger contact” case is dealt with last because of its somewhat 
peculiar nature. This a one-sided point contact with friction such that a moment friction force exists 
about the normal to the object tangent plane at the contact point, prohibiting rotational motion about 
that normal [16]. All allowable motions are rotations about any axis in the contact tangent plane. Since 
this contact is one-sided, it can be broken if an appropriate contact normal force is not maintained. There 
exists no equivalent 2-dof rotational lower kinematic pair (and hence two-sided) contact [23]. The soft 
finger contact has the nice modeling feature that it is a one-sided point contact for which a two-finger 
grasp (i.e. two soft finger point contacts) of the held object is statically stable [16]. This is in contrast to 
the point contact with friction considered in Case 2, for which at least three contact points are needed for 
a stable grasp. Let h] (0) G R 3 be the unit 3-vector normal to the contact tangent plane at contact point 
Oi . Let h 2 (0), h 3 ( 0) G R 3 be mutually orthogonal 3-vectors which are orthogonal to h}( 0) and lie in the 
contact tangent plane. Admissible relative angular velocities at O, are given by 

tout = h1(0)Auj? + fc?( 0)Aw? 

= I*?(0).A?(0)1 (£"|) . 

Define W<(0) = col[Auf, tout] G R 2 , W{ 0) = co/[W,(0)], h,(0) = [h 2 (0) , h 3 (0)} G R 3x2 , and h(0) = 
dia^Jhj(O)]. For the configuration parameters take, as in Case 2, 0(0) = g(0) G R 4t (Euler parameters, 
m = 4) or 0(0) = /3(0) G R 3t (Euler angles, m = 3), i.e., take a representation for the entire rotation group. 
This may seem surprising since n g = 2, but it can be shown that even though rotations about the contact 
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normal axe not allowed, every possible contact attitude is attainable [26]. In [21,22] or [24], the relationship 
0(0) = J F[0(O)]Ao; is given in detail, which with Ao;+ = ^(OjW^O) gives 0(0) = F[0(O)]h(O)I^(O). Thus 
F of (a.4) is given by F = Fh(0 ). 

Now that (a.2)-(a.6) has been explained in some detail, consider the force and acceleration conditions 
which must hold at the contact points. Since AV r+ represents a motion at the contact point which is freely 
allowed, then 

f T (0)AV+ = 0 = / r (0)ff T (0)^(0) for all W( 0). 

Equivalently, 

H{0)f(0) = 0. (a. 7) 

(In the terminology of [16], /( 0) and AV+ are “reciprocal”). 

Differentiation of (a.6) leads to 

a~ (0) = a + (0) + F r (0)W(0) + H T {0)W{0) (a.8) 

(a.9) 
(a. 10) 


Furthermore, it must be true that 

a + (0 ) = J0 + j6 
a - (0) = Aa(C) + a(C). 


72 



APPENDIX B: THE CLOSED-CHAIN FORWARD DYNAMICS PROBLEM 
FOR NON-FULLY-CONSTRAINED GRASPS 

In this appendix, the forward dynamics equations for a closed-chain system with nonrigid, nonslipping 
contacts of the type given in Appendix A are derived. The notation of Appendix A is used freely here 
without additional explanation. 

Together with the manipulator dynamics M9+C + J T f(0) = T, and the held object dynamics M(C)a(C) + 
b(C ) = A T f( 0), (a.7)-(a.!0) give the nonrigid grasp closed-chain dynamics: 


M9 + C + J T f{ 0) = T 

(6.1a) 

M(C)a(C) + 6(C ) = A T f{ 0) 

(6.16) 

mm = o 

(6.1c) 

a+ (o) = jo + jb 

(b.ld) 

a”(0) = Aa(C) + a(C) 

(6.1c) 

= a + (0) + H t (0)W(0) + R r (0)W(0). 

(6.1/) 


With rigid grasps H( 0) = H( 0) = 0 and (b.l) reduces to (2.10). 

The forward dynamics problem for (b.l) is to produce [0, W(0), /(0)] from [0, 0, 0(0), W (0), T]. Note 
that having /( 0) enables 9 to be computed from (b.la) via the techniques developed in the main body of 
this paper, and thus the issues of obtaining / (0) and W (0) are the novel ones. 

From (b.la), 9 = X -1 [T - J T f( 0) - C], which with (b.ld) gives 

a+{0) = JM- 1 [T-J T f{0)-C] + j9 (6.2) 

From (b.lb), a(C) = M~ 1 (C)[A r /( 0) - 6(C)], which with (b.le) gives 

a"(0) = AM~ 1 (C)A T f( 0) - AM _1 (C)6(C) + a(C). (6.3) 

Eqs. (b.lf), (b.2), and (b.3) result in 

n = JM~ 1 T + AM~ 1 (C)A t (6.4a) 

n/(0) = JM~'T + H t (0)W{0) + Cx (6.46) 
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(?! = AM~ l (C)b(C!) + J9 + ff T (0)W(0) - a(C) - JM~ X C (6.4c) 

Note that with H( 0) — H{0) = 0, then (b.4) becomes (3.6b). Under the assumption that Q is invertible 


/( 0) = n-'iJM-'T + G i) + n -1 JI T Fk(0), 


which with (b.lc) gives 


[ J H'(o)n- 1 J cr r (o)]w(o) = -ff(o)n _1 jm~ x t - H{o)n~ l G u (6.5a) 

or 

w(o) = -[fl r (o)n- 1 F r (o)]- 1 {fl'(o)n“ 1 JM- 1 r + F(o)n- x Gi}. (6.56) 

(b.l), (b.4), and (b.5) combine to give the following solution to the closed-chain forward dynamics 
problem. 

w(o) = ~[h{ o)n -1 ff r (o)] -1 [ffn -1 j.M -1 r + ffffln^Gi] (6.6a) 

/( 0) = Q-'IJM-'T + H t {0)W{0) + Gi] (6.66) 

M0 + C + J T f{ 0) = T. (6.6c) 


Inversion of H(0)Q 1 H T (0) € R n a tXn 9 t j s an 0(£ 3 ) operation, and finding the nonrigid grasp closed-chain 
forward dynamics via the recursive techniques of this paper remains O(n) + 0(£ 3 ). 


Together, Equations (b.6a) and (b.6b) give 

! 

/(0) = n(JX- 1 T + Gi) (6.7a) 

n = n _1 - n- 1 fl' r (o)[F(o)n- 1 F r (o)]- 1 fl-(o)n- 1 (6.76) 

which can be compared to (3.6b). With (b.7), (b.6c) is 

! 

9 = (M' 1 - - M~ 1 J T nG 1 - M~ X C. (6.8) 

| 

The combination of (b.4), (b.6a), (b.7) and (b.8) gives 

Gj = AM -1 (G)6(G) + J0 + H t {0)W{0) - a(C) - JH^C (6.9a) 

n = + AM~ 1 {C)A t (6.96) 
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n = n _1 - n- 1 H T (o)[H{o)n- 1 H T (o)}- 1 H(o)n ~ 1 (6.9c) 

/(O) = n(JM _1 T + Gi) (b.9d) 

9 = (AT 1 - - X- 1 J r nG 1 - M^C {b. 9c) 

w(o) = -[ff(o)n- 1 ^ T (o)]- 1 [ff(o)n- 1 jm-'t + Ffojn^Gi] ( 6 . 9 /) 


Eqs. (b.9) give the complete analytical solution to the closed-chain forward dynamics problem of several 
rigid link manipulators holding a rigid object with nonrigid grasps. 

From (b.3), (b.ld) and (b.lf) follows 

<*-(0) - a + (0) = AM~ 1 {C)A T f{ 0) - AM -1 (C)6(C) + a(C) - J9 - J9 

= H t (0)W(0) + H t (0)W(0) 

which can be rewritten as 

ff r (0)W(0) + J9 = AM* 1 (C)A t f (0) - Gi + JM-'C. 

Premultiplying by H( 0)n _1 , 

fr(o)n _1 ff T (o)^(o) + ff(o)n _1 j0 = #(o)fr 1 AM -1 (c)A T /(o) + - g^. 

With (b.9d), this becomes 

H( 0 )n- 1 H T ( 0 )W( 0 ) + H{0)n~ 1 J9 

= H( 0 )n- 1 AM- 1 (C)A T nJM~ 1 T + £r( 0 )n- 1 AAf- 1 (G)A r nG 1 

+H(0)n- 1 (JM~ 1 C-G 1 ) 

= ff(o)n- 1 AM“ 1 (G)A T njAr 1 r + G 2 ( 6 . 10 ) 

where 

G 2 = ^( 0 )n- 1 {(AM" 1 (C')A T n - J)Gi + JM-'C}. (6.11) 

With (b.6b), (b.6c) can be written as 

mo + J T n- 1 tf T (o)w(o) = (j - j T n _1 jx _1 )r - J T n ~ 1 g 1 - c 
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(I - JTQ-'JM-^T + Ga, 


( 6 . 12 ) 


where 

G 3 = ~J T n~ 1 G 1 -C. (6.13) 

Equations (b.10) and (b.12) combine to give 

( H(0)n~ 1 H T (0) H{0)n^J\ (W{ 0 )\ _ f H(0)n- 1 AM- 1 {C)A T UJM- 1 \ „ (g 2 \ 

^ j T n~ , H T (o) w ) v i ) ^ 1 j 1 + \g s ) 

(6.14) 

Equation (b.14) describes the dynamical behavior of (9, 9) and [©(0), W (0)]. The forward dynamics solution 
to (b.14) is given by (b.9e) and (b.9f). 
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TABLE 2.1 DEFINITION OF SYMBOLS 


.M, = mass operator of arm » 

Ci = coriolis and centrifugal terms for arm » 

Ji = Jacobian operator of arm i 
Oi = vector of joint variables of arm $ 

Ti = generalized joint forces or moments of arm i 
N = number of joints (links) of arm i 
t = number of arms (grasp points) 
n = IN = total number of manipulator links and joints 

Xj(0) = orientation and location of arm i tip. Usually Aj(0) G R 6 or A,(0) € R 3 x 50(3) 

V{( 0) = co/fw^O), v,(0)] G R 6 = spatial velocity of arm * tip 
a,(0) = co/[a;,(0),t>,(0)] G R 6 = spatial accelerations of arm i tip 

/, (0) = col[Ni(0), Fj(0)] G R 6 = spatial contact force imparted to object at contact point Oi by tip 

of arm t 

Oi = contact point of arm » tip with held object 
C = point fixed with respect to held object 
M(C) = spatial mass of held object referenced to C 
b(C) = “bias spatial force” acting on held object at C 

f(C) = col[N(C), F(C)\ G R e = resultant spatial force on held object at C due to grasp contact forces 

X{C ) = orientation and location of held object at C 

V (C) = col[<jj{C),v(C)\ G R 6 = spatial velocity of object at C 

a(C) = col[u>(C),v(C)\ G i? 6 = spatial acceleration of object at C 

£(C, Oi) = 3-vector from point C to point Oi 
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